diff options
author | ubit | 2012-10-12 01:40:47 +0200 |
---|---|---|
committer | ubit | 2012-10-12 01:40:47 +0200 |
commit | 711a736097798af6aa597e5ee27ff7be52052368 (patch) | |
tree | ffbd304cfaf7911abcff5c70f89902a53cb72463 | |
parent | Merge branch 'ubitwork' of ssh://3dhosting.de/var/git/careminster into ubitwork (diff) | |
parent | commit the right files! (diff) | |
download | opensim-SC-711a736097798af6aa597e5ee27ff7be52052368.zip opensim-SC-711a736097798af6aa597e5ee27ff7be52052368.tar.gz opensim-SC-711a736097798af6aa597e5ee27ff7be52052368.tar.bz2 opensim-SC-711a736097798af6aa597e5ee27ff7be52052368.tar.xz |
Merge branch 'ubitwork' of ssh://3dhosting.de/var/git/careminster into ubitwork
-rw-r--r-- | OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 5345 | ||||
-rw-r--r-- | OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 5155 |
2 files changed, 4763 insertions, 5737 deletions
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index eaf0d0a..f083d38 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | |||
@@ -25,6 +25,11 @@ | |||
25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. | 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
26 | */ | 26 | */ |
27 | 27 | ||
28 | /* Revision 2011/12 by Ubit Umarov | ||
29 | * | ||
30 | * | ||
31 | */ | ||
32 | |||
28 | /* | 33 | /* |
29 | * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces | 34 | * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces |
30 | * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: | 35 | * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: |
@@ -48,250 +53,251 @@ using System.Runtime.InteropServices; | |||
48 | using System.Threading; | 53 | using System.Threading; |
49 | using log4net; | 54 | using log4net; |
50 | using OpenMetaverse; | 55 | using OpenMetaverse; |
51 | using Ode.NET; | 56 | using OdeAPI; |
52 | using OpenSim.Framework; | 57 | using OpenSim.Framework; |
53 | using OpenSim.Region.Physics.Manager; | 58 | using OpenSim.Region.Physics.Manager; |
54 | 59 | ||
55 | namespace OpenSim.Region.Physics.OdePlugin | 60 | namespace OpenSim.Region.Physics.OdePlugin |
56 | { | 61 | { |
57 | /// <summary> | ||
58 | /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves. | ||
59 | /// </summary> | ||
60 | public class OdePrim : PhysicsActor | 62 | public class OdePrim : PhysicsActor |
61 | { | 63 | { |
62 | private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); | 64 | private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); |
63 | 65 | ||
64 | private bool m_isphysical; | 66 | private bool m_isphysical; |
67 | private bool m_fakeisphysical; | ||
68 | private bool m_isphantom; | ||
69 | private bool m_fakeisphantom; | ||
70 | internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively | ||
71 | private bool m_fakeisVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively | ||
65 | 72 | ||
66 | public int ExpectedCollisionContacts { get { return m_expectedCollisionContacts; } } | 73 | protected bool m_building; |
67 | private int m_expectedCollisionContacts = 0; | 74 | protected bool m_forcePosOrRotation; |
75 | private bool m_iscolliding; | ||
68 | 76 | ||
69 | /// <summary> | 77 | internal bool m_isSelected; |
70 | /// Is this prim subject to physics? Even if not, it's still solid for collision purposes. | 78 | private bool m_delaySelect; |
71 | /// </summary> | 79 | private bool m_lastdoneSelected; |
72 | public override bool IsPhysical | 80 | internal bool m_outbounds; |
73 | { | 81 | |
74 | get { return m_isphysical; } | 82 | private Quaternion m_lastorientation; |
75 | set | 83 | private Quaternion _orientation; |
76 | { | ||
77 | m_isphysical = value; | ||
78 | if (!m_isphysical) // Zero the remembered last velocity | ||
79 | m_lastVelocity = Vector3.Zero; | ||
80 | } | ||
81 | } | ||
82 | 84 | ||
83 | private Vector3 _position; | 85 | private Vector3 _position; |
84 | private Vector3 _velocity; | 86 | private Vector3 _velocity; |
85 | private Vector3 _torque; | 87 | private Vector3 _torque; |
86 | private Vector3 m_lastVelocity; | 88 | private Vector3 m_lastVelocity; |
87 | private Vector3 m_lastposition; | 89 | private Vector3 m_lastposition; |
88 | private Quaternion m_lastorientation = new Quaternion(); | ||
89 | private Vector3 m_rotationalVelocity; | 90 | private Vector3 m_rotationalVelocity; |
90 | private Vector3 _size; | 91 | private Vector3 _size; |
91 | private Vector3 _acceleration; | 92 | private Vector3 _acceleration; |
92 | // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f); | ||
93 | private Quaternion _orientation; | ||
94 | private Vector3 m_taintposition; | ||
95 | private Vector3 m_taintsize; | ||
96 | private Vector3 m_taintVelocity; | ||
97 | private Vector3 m_taintTorque; | ||
98 | private Quaternion m_taintrot; | ||
99 | private Vector3 m_angularlock = Vector3.One; | 93 | private Vector3 m_angularlock = Vector3.One; |
100 | private Vector3 m_taintAngularLock = Vector3.One; | 94 | private IntPtr Amotor; |
101 | private IntPtr Amotor = IntPtr.Zero; | ||
102 | 95 | ||
103 | private object m_assetsLock = new object(); | 96 | private Vector3 m_force; |
104 | private bool m_assetFailed = false; | 97 | private Vector3 m_forceacc; |
98 | private Vector3 m_angularForceacc; | ||
99 | |||
100 | private float m_invTimeStep; | ||
101 | private float m_timeStep; | ||
105 | 102 | ||
106 | private Vector3 m_PIDTarget; | 103 | private Vector3 m_PIDTarget; |
107 | private float m_PIDTau; | 104 | private float m_PIDTau; |
108 | private float PID_D = 35f; | ||
109 | private float PID_G = 25f; | ||
110 | private bool m_usePID; | 105 | private bool m_usePID; |
111 | 106 | ||
112 | // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), | ||
113 | // and are for non-VEHICLES only. | ||
114 | |||
115 | private float m_PIDHoverHeight; | 107 | private float m_PIDHoverHeight; |
116 | private float m_PIDHoverTau; | 108 | private float m_PIDHoverTau; |
117 | private bool m_useHoverPID; | 109 | private bool m_useHoverPID; |
118 | private PIDHoverType m_PIDHoverType = PIDHoverType.Ground; | 110 | private PIDHoverType m_PIDHoverType; |
119 | private float m_targetHoverHeight; | 111 | private float m_targetHoverHeight; |
120 | private float m_groundHeight; | 112 | private float m_groundHeight; |
121 | private float m_waterHeight; | 113 | private float m_waterHeight; |
122 | private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. | 114 | private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. |
123 | 115 | ||
124 | // private float m_tensor = 5f; | 116 | private int body_autodisable_frames; |
125 | private int body_autodisable_frames = 20; | 117 | public int bodydisablecontrol; |
126 | |||
127 | 118 | ||
128 | private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom | ||
129 | | CollisionCategories.Space | ||
130 | | CollisionCategories.Body | ||
131 | | CollisionCategories.Character | ||
132 | ); | ||
133 | private bool m_taintshape; | ||
134 | private bool m_taintPhysics; | ||
135 | private bool m_collidesLand = true; | ||
136 | private bool m_collidesWater; | ||
137 | 119 | ||
138 | // Default we're a Geometry | 120 | // Default we're a Geometry |
139 | private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); | 121 | private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); |
122 | // Default colide nonphysical don't try to colide with anything | ||
123 | private const CollisionCategories m_default_collisionFlagsNotPhysical = 0; | ||
124 | |||
125 | private const CollisionCategories m_default_collisionFlagsPhysical = (CollisionCategories.Geom | | ||
126 | CollisionCategories.Character | | ||
127 | CollisionCategories.Land | | ||
128 | CollisionCategories.VolumeDtc); | ||
129 | |||
130 | // private bool m_collidesLand = true; | ||
131 | private bool m_collidesWater; | ||
132 | // public bool m_returnCollisions; | ||
133 | |||
134 | private bool m_NoColide; // for now only for internal use for bad meshs | ||
135 | |||
140 | 136 | ||
141 | // Default, Collide with Other Geometries, spaces and Bodies | 137 | // Default, Collide with Other Geometries, spaces and Bodies |
142 | private CollisionCategories m_collisionFlags = m_default_collisionFlags; | 138 | private CollisionCategories m_collisionFlags = m_default_collisionFlagsNotPhysical; |
143 | 139 | ||
144 | public bool m_taintremove { get; private set; } | 140 | public bool m_disabled; |
145 | public bool m_taintdisable { get; private set; } | ||
146 | internal bool m_disabled; | ||
147 | public bool m_taintadd { get; private set; } | ||
148 | public bool m_taintselected { get; private set; } | ||
149 | public bool m_taintCollidesWater { get; private set; } | ||
150 | 141 | ||
151 | private bool m_taintforce = false; | 142 | private uint m_localID; |
152 | private bool m_taintaddangularforce = false; | ||
153 | private Vector3 m_force; | ||
154 | private List<Vector3> m_forcelist = new List<Vector3>(); | ||
155 | private List<Vector3> m_angularforcelist = new List<Vector3>(); | ||
156 | 143 | ||
144 | private IMesh m_mesh; | ||
145 | private object m_meshlock = new object(); | ||
157 | private PrimitiveBaseShape _pbs; | 146 | private PrimitiveBaseShape _pbs; |
158 | private OdeScene _parent_scene; | ||
159 | 147 | ||
160 | /// <summary> | 148 | private UUID? m_assetID; |
161 | /// The physics space which contains prim geometries | 149 | private MeshState m_meshState; |
162 | /// </summary> | 150 | |
163 | public IntPtr m_targetSpace = IntPtr.Zero; | 151 | public OdeScene _parent_scene; |
164 | 152 | ||
165 | /// <summary> | 153 | /// <summary> |
166 | /// The prim geometry, used for collision detection. | 154 | /// The physics space which contains prim geometry |
167 | /// </summary> | 155 | /// </summary> |
168 | /// <remarks> | 156 | public IntPtr m_targetSpace; |
169 | /// This is never null except for a brief period when the geometry needs to be replaced (due to resizing or | ||
170 | /// mesh change) or when the physical prim is being removed from the scene. | ||
171 | /// </remarks> | ||
172 | public IntPtr prim_geom { get; private set; } | ||
173 | 157 | ||
174 | public IntPtr _triMeshData { get; private set; } | 158 | public IntPtr prim_geom; |
159 | public IntPtr _triMeshData; | ||
175 | 160 | ||
176 | private IntPtr _linkJointGroup = IntPtr.Zero; | ||
177 | private PhysicsActor _parent; | 161 | private PhysicsActor _parent; |
178 | private PhysicsActor m_taintparent; | ||
179 | 162 | ||
180 | private List<OdePrim> childrenPrim = new List<OdePrim>(); | 163 | private List<OdePrim> childrenPrim = new List<OdePrim>(); |
181 | 164 | ||
182 | private bool iscolliding; | 165 | public float m_collisionscore; |
183 | private bool m_isSelected; | 166 | private int m_colliderfilter = 0; |
184 | |||
185 | internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively | ||
186 | |||
187 | private bool m_throttleUpdates; | ||
188 | private int throttleCounter; | ||
189 | public int m_interpenetrationcount { get; private set; } | ||
190 | internal float m_collisionscore; | ||
191 | public int m_roundsUnderMotionThreshold { get; private set; } | ||
192 | private int m_crossingfailures; | ||
193 | 167 | ||
194 | public bool outofBounds { get; private set; } | 168 | public IntPtr collide_geom; // for objects: geom if single prim space it linkset |
195 | private float m_density = 10.000006836f; // Aluminum g/cm3; | ||
196 | 169 | ||
197 | public bool _zeroFlag { get; private set; } | 170 | private float m_density; |
171 | private byte m_shapetype; | ||
172 | public bool _zeroFlag; | ||
198 | private bool m_lastUpdateSent; | 173 | private bool m_lastUpdateSent; |
199 | 174 | ||
200 | public IntPtr Body = IntPtr.Zero; | 175 | public IntPtr Body; |
176 | |||
201 | private Vector3 _target_velocity; | 177 | private Vector3 _target_velocity; |
202 | private d.Mass pMass; | ||
203 | 178 | ||
204 | private int m_eventsubscription; | 179 | public Vector3 m_OBBOffset; |
205 | private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); | 180 | public Vector3 m_OBB; |
181 | public float primOOBradiusSQ; | ||
206 | 182 | ||
207 | /// <summary> | 183 | private bool m_hasOBB = true; |
208 | /// Signal whether there were collisions on the previous frame, so we know if we need to send the | 184 | |
209 | /// empty CollisionEventsThisFrame to the prim so that it can detect the end of a collision. | 185 | private float m_physCost; |
210 | /// </summary> | 186 | private float m_streamCost; |
211 | /// <remarks> | 187 | |
212 | /// This is probably a temporary measure, pending storing this information consistently in CollisionEventUpdate itself. | 188 | public d.Mass primdMass; // prim inertia information on it's own referencial |
213 | /// </remarks> | 189 | float primMass; // prim own mass |
214 | private bool m_collisionsOnPreviousFrame; | 190 | float primVolume; // prim own volume; |
191 | float _mass; // object mass acording to case | ||
192 | |||
193 | public int givefakepos; | ||
194 | private Vector3 fakepos; | ||
195 | public int givefakeori; | ||
196 | private Quaternion fakeori; | ||
215 | 197 | ||
216 | private IntPtr m_linkJoint = IntPtr.Zero; | 198 | private int m_eventsubscription; |
199 | private int m_cureventsubscription; | ||
200 | private CollisionEventUpdate CollisionEventsThisFrame = null; | ||
201 | private bool SentEmptyCollisionsEvent; | ||
217 | 202 | ||
218 | internal volatile bool childPrim; | 203 | public volatile bool childPrim; |
219 | 204 | ||
220 | private ODEDynamics m_vehicle; | 205 | public ODEDynamics m_vehicle; |
221 | 206 | ||
222 | internal int m_material = (int)Material.Wood; | 207 | internal int m_material = (int)Material.Wood; |
208 | private float mu; | ||
209 | private float bounce; | ||
223 | 210 | ||
224 | public OdePrim( | 211 | /// <summary> |
225 | String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, | 212 | /// Is this prim subject to physics? Even if not, it's still solid for collision purposes. |
226 | Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical) | 213 | /// </summary> |
214 | public override bool IsPhysical // this is not reliable for internal use | ||
227 | { | 215 | { |
228 | Name = primName; | 216 | get { return m_fakeisphysical; } |
229 | m_vehicle = new ODEDynamics(); | 217 | set |
230 | //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned); | ||
231 | |||
232 | if (!pos.IsFinite()) | ||
233 | { | 218 | { |
234 | pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), | 219 | m_fakeisphysical = value; // we show imediatly to outside that we changed physical |
235 | parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); | 220 | // and also to stop imediatly some updates |
236 | m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name); | 221 | // but real change will only happen in taintprocessing |
237 | } | ||
238 | _position = pos; | ||
239 | m_taintposition = pos; | ||
240 | PID_D = parent_scene.bodyPIDD; | ||
241 | PID_G = parent_scene.bodyPIDG; | ||
242 | m_density = parent_scene.geomDefaultDensity; | ||
243 | // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; | ||
244 | body_autodisable_frames = parent_scene.bodyFramesAutoDisable; | ||
245 | 222 | ||
246 | prim_geom = IntPtr.Zero; | 223 | if (!value) // Zero the remembered last velocity |
224 | m_lastVelocity = Vector3.Zero; | ||
225 | AddChange(changes.Physical, value); | ||
226 | } | ||
227 | } | ||
247 | 228 | ||
248 | if (!pos.IsFinite()) | 229 | public override bool IsVolumeDtc |
230 | { | ||
231 | get { return m_fakeisVolumeDetect; } | ||
232 | set | ||
249 | { | 233 | { |
250 | size = new Vector3(0.5f, 0.5f, 0.5f); | 234 | m_fakeisVolumeDetect = value; |
251 | m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name); | 235 | AddChange(changes.VolumeDtc, value); |
252 | } | 236 | } |
237 | } | ||
253 | 238 | ||
254 | if (size.X <= 0) size.X = 0.01f; | 239 | public override bool Phantom // this is not reliable for internal use |
255 | if (size.Y <= 0) size.Y = 0.01f; | 240 | { |
256 | if (size.Z <= 0) size.Z = 0.01f; | 241 | get { return m_fakeisphantom; } |
257 | 242 | set | |
258 | _size = size; | 243 | { |
259 | m_taintsize = _size; | 244 | m_fakeisphantom = value; |
245 | AddChange(changes.Phantom, value); | ||
246 | } | ||
247 | } | ||
260 | 248 | ||
261 | if (!QuaternionIsFinite(rotation)) | 249 | public override bool Building // this is not reliable for internal use |
250 | { | ||
251 | get { return m_building; } | ||
252 | set | ||
262 | { | 253 | { |
263 | rotation = Quaternion.Identity; | 254 | if (value) |
264 | m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name); | 255 | m_building = true; |
256 | AddChange(changes.building, value); | ||
265 | } | 257 | } |
258 | } | ||
266 | 259 | ||
267 | _orientation = rotation; | 260 | public override void getContactData(ref ContactData cdata) |
268 | m_taintrot = _orientation; | 261 | { |
269 | _pbs = pbs; | 262 | cdata.mu = mu; |
263 | cdata.bounce = bounce; | ||
270 | 264 | ||
271 | _parent_scene = parent_scene; | 265 | // cdata.softcolide = m_softcolide; |
272 | m_targetSpace = (IntPtr)0; | 266 | cdata.softcolide = false; |
273 | 267 | ||
274 | if (pos.Z < 0) | 268 | if (m_isphysical) |
275 | { | 269 | { |
276 | IsPhysical = false; | 270 | ODEDynamics veh; |
271 | if (_parent != null) | ||
272 | veh = ((OdePrim)_parent).m_vehicle; | ||
273 | else | ||
274 | veh = m_vehicle; | ||
275 | |||
276 | if (veh != null && veh.Type != Vehicle.TYPE_NONE) | ||
277 | cdata.mu *= veh.FrictionFactor; | ||
278 | // cdata.mu *= 0; | ||
277 | } | 279 | } |
278 | else | 280 | } |
281 | |||
282 | public override float PhysicsCost | ||
283 | { | ||
284 | get | ||
279 | { | 285 | { |
280 | IsPhysical = pisPhysical; | 286 | return m_physCost; |
281 | // If we're physical, we need to be in the master space for now. | ||
282 | // linksets *should* be in a space together.. but are not currently | ||
283 | if (IsPhysical) | ||
284 | m_targetSpace = _parent_scene.space; | ||
285 | } | 287 | } |
288 | } | ||
286 | 289 | ||
287 | m_taintadd = true; | 290 | public override float StreamCost |
288 | m_assetFailed = false; | 291 | { |
289 | _parent_scene.AddPhysicsActorTaint(this); | 292 | get |
293 | { | ||
294 | return m_streamCost; | ||
295 | } | ||
290 | } | 296 | } |
291 | 297 | ||
292 | public override int PhysicsActorType | 298 | public override int PhysicsActorType |
293 | { | 299 | { |
294 | get { return (int) ActorTypes.Prim; } | 300 | get { return (int)ActorTypes.Prim; } |
295 | set { return; } | 301 | set { return; } |
296 | } | 302 | } |
297 | 303 | ||
@@ -301,6 +307,23 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
301 | set { return; } | 307 | set { return; } |
302 | } | 308 | } |
303 | 309 | ||
310 | public override uint LocalID | ||
311 | { | ||
312 | get { return m_localID; } | ||
313 | set { m_localID = value; } | ||
314 | } | ||
315 | |||
316 | public override PhysicsActor ParentActor | ||
317 | { | ||
318 | get | ||
319 | { | ||
320 | if (childPrim) | ||
321 | return _parent; | ||
322 | else | ||
323 | return (PhysicsActor)this; | ||
324 | } | ||
325 | } | ||
326 | |||
304 | public override bool Grabbed | 327 | public override bool Grabbed |
305 | { | 328 | { |
306 | set { return; } | 329 | set { return; } |
@@ -310,2383 +333,3063 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
310 | { | 333 | { |
311 | set | 334 | set |
312 | { | 335 | { |
313 | // This only makes the object not collidable if the object | 336 | if (value) |
314 | // is physical or the object is modified somehow *IN THE FUTURE* | 337 | m_isSelected = value; // if true set imediatly to stop moves etc |
315 | // without this, if an avatar selects prim, they can walk right | 338 | AddChange(changes.Selected, value); |
316 | // through it while it's selected | 339 | } |
317 | m_collisionscore = 0; | 340 | } |
318 | 341 | ||
319 | if ((IsPhysical && !_zeroFlag) || !value) | 342 | public override bool Flying |
343 | { | ||
344 | // no flying prims for you | ||
345 | get { return false; } | ||
346 | set { } | ||
347 | } | ||
348 | |||
349 | public override bool IsColliding | ||
350 | { | ||
351 | get { return m_iscolliding; } | ||
352 | set | ||
353 | { | ||
354 | if (value) | ||
320 | { | 355 | { |
321 | m_taintselected = value; | 356 | m_colliderfilter += 2; |
322 | _parent_scene.AddPhysicsActorTaint(this); | 357 | if (m_colliderfilter > 2) |
358 | m_colliderfilter = 2; | ||
323 | } | 359 | } |
324 | else | 360 | else |
325 | { | 361 | { |
326 | m_taintselected = value; | 362 | m_colliderfilter--; |
327 | m_isSelected = value; | 363 | if (m_colliderfilter < 0) |
364 | m_colliderfilter = 0; | ||
328 | } | 365 | } |
329 | 366 | ||
330 | if (m_isSelected) | 367 | if (m_colliderfilter == 0) |
331 | disableBodySoft(); | 368 | m_iscolliding = false; |
369 | else | ||
370 | m_iscolliding = true; | ||
332 | } | 371 | } |
333 | } | 372 | } |
334 | 373 | ||
335 | /// <summary> | 374 | public override bool CollidingGround |
336 | /// Set a new geometry for this prim. | ||
337 | /// </summary> | ||
338 | /// <param name="geom"></param> | ||
339 | private void SetGeom(IntPtr geom) | ||
340 | { | 375 | { |
341 | prim_geom = geom; | 376 | get { return false; } |
342 | //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); | 377 | set { return; } |
378 | } | ||
379 | |||
380 | public override bool CollidingObj | ||
381 | { | ||
382 | get { return false; } | ||
383 | set { return; } | ||
384 | } | ||
385 | |||
343 | 386 | ||
344 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | 387 | public override bool ThrottleUpdates {get;set;} |
345 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | 388 | |
389 | public override bool Stopped | ||
390 | { | ||
391 | get { return _zeroFlag; } | ||
392 | } | ||
346 | 393 | ||
347 | _parent_scene.geom_name_map[prim_geom] = Name; | 394 | public override Vector3 Position |
348 | _parent_scene.actor_name_map[prim_geom] = this; | 395 | { |
396 | get | ||
397 | { | ||
398 | if (givefakepos > 0) | ||
399 | return fakepos; | ||
400 | else | ||
401 | return _position; | ||
402 | } | ||
349 | 403 | ||
350 | if (childPrim) | 404 | set |
351 | { | 405 | { |
352 | if (_parent != null && _parent is OdePrim) | 406 | fakepos = value; |
353 | { | 407 | givefakepos++; |
354 | OdePrim parent = (OdePrim)_parent; | 408 | AddChange(changes.Position, value); |
355 | //Console.WriteLine("SetGeom calls ChildSetGeom"); | ||
356 | parent.ChildSetGeom(this); | ||
357 | } | ||
358 | } | 409 | } |
359 | //m_log.Warn("Setting Geom to: " + prim_geom); | ||
360 | } | 410 | } |
361 | 411 | ||
362 | private void enableBodySoft() | 412 | public override Vector3 Size |
363 | { | 413 | { |
364 | if (!childPrim) | 414 | get { return _size; } |
415 | set | ||
365 | { | 416 | { |
366 | if (IsPhysical && Body != IntPtr.Zero) | 417 | if (value.IsFinite()) |
367 | { | 418 | { |
368 | d.BodyEnable(Body); | 419 | _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, value, m_shapetype); |
369 | if (m_vehicle.Type != Vehicle.TYPE_NONE) | 420 | } |
370 | m_vehicle.Enable(Body, _parent_scene); | 421 | else |
422 | { | ||
423 | m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name); | ||
371 | } | 424 | } |
372 | |||
373 | m_disabled = false; | ||
374 | } | 425 | } |
375 | } | 426 | } |
376 | 427 | ||
377 | private void disableBodySoft() | 428 | public override float Mass |
378 | { | 429 | { |
379 | m_disabled = true; | 430 | get { return primMass; } |
380 | |||
381 | if (IsPhysical && Body != IntPtr.Zero) | ||
382 | { | ||
383 | d.BodyDisable(Body); | ||
384 | } | ||
385 | } | 431 | } |
386 | 432 | ||
387 | /// <summary> | 433 | public override Vector3 Force |
388 | /// Make a prim subject to physics. | ||
389 | /// </summary> | ||
390 | private void enableBody() | ||
391 | { | 434 | { |
392 | // Don't enable this body if we're a child prim | 435 | get { return m_force; } |
393 | // this should be taken care of in the parent function not here | 436 | set |
394 | if (!childPrim) | ||
395 | { | 437 | { |
396 | // Sets the geom to a body | 438 | if (value.IsFinite()) |
397 | Body = d.BodyCreate(_parent_scene.world); | ||
398 | |||
399 | setMass(); | ||
400 | d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); | ||
401 | d.Quaternion myrot = new d.Quaternion(); | ||
402 | myrot.X = _orientation.X; | ||
403 | myrot.Y = _orientation.Y; | ||
404 | myrot.Z = _orientation.Z; | ||
405 | myrot.W = _orientation.W; | ||
406 | d.BodySetQuaternion(Body, ref myrot); | ||
407 | d.GeomSetBody(prim_geom, Body); | ||
408 | m_collisionCategories |= CollisionCategories.Body; | ||
409 | m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); | ||
410 | |||
411 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | ||
412 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
413 | |||
414 | d.BodySetAutoDisableFlag(Body, true); | ||
415 | d.BodySetAutoDisableSteps(Body, body_autodisable_frames); | ||
416 | |||
417 | // disconnect from world gravity so we can apply buoyancy | ||
418 | d.BodySetGravityMode (Body, false); | ||
419 | |||
420 | m_interpenetrationcount = 0; | ||
421 | m_collisionscore = 0; | ||
422 | m_disabled = false; | ||
423 | |||
424 | // The body doesn't already have a finite rotation mode set here | ||
425 | if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0.0f)) && _parent == null) | ||
426 | { | 439 | { |
427 | createAMotor(m_angularlock); | 440 | AddChange(changes.Force, value); |
428 | } | 441 | } |
429 | if (m_vehicle.Type != Vehicle.TYPE_NONE) | 442 | else |
430 | { | 443 | { |
431 | m_vehicle.Enable(Body, _parent_scene); | 444 | m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name); |
432 | } | 445 | } |
433 | |||
434 | _parent_scene.ActivatePrim(this); | ||
435 | } | 446 | } |
436 | } | 447 | } |
437 | 448 | ||
438 | #region Mass Calculation | 449 | public override void SetVolumeDetect(int param) |
439 | |||
440 | private float CalculateMass() | ||
441 | { | 450 | { |
442 | float volume = _size.X * _size.Y * _size.Z; // default | 451 | m_fakeisVolumeDetect = (param != 0); |
443 | float tmp; | 452 | AddChange(changes.VolumeDtc, m_fakeisVolumeDetect); |
453 | } | ||
444 | 454 | ||
445 | float returnMass = 0; | 455 | public override Vector3 GeometricCenter |
446 | float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; | 456 | { |
447 | float hollowVolume = hollowAmount * hollowAmount; | 457 | // this is not real geometric center but a average of positions relative to root prim acording to |
448 | 458 | // http://wiki.secondlife.com/wiki/llGetGeometricCenter | |
449 | switch (_pbs.ProfileShape) | 459 | // ignoring tortured prims details since sl also seems to ignore |
460 | // so no real use in doing it on physics | ||
461 | get | ||
450 | { | 462 | { |
451 | case ProfileShape.Square: | 463 | return Vector3.Zero; |
452 | // default box | 464 | } |
453 | 465 | } | |
454 | if (_pbs.PathCurve == (byte)Extrusion.Straight) | ||
455 | { | ||
456 | if (hollowAmount > 0.0) | ||
457 | { | ||
458 | switch (_pbs.HollowShape) | ||
459 | { | ||
460 | case HollowShape.Square: | ||
461 | case HollowShape.Same: | ||
462 | break; | ||
463 | |||
464 | case HollowShape.Circle: | ||
465 | |||
466 | hollowVolume *= 0.78539816339f; | ||
467 | break; | ||
468 | |||
469 | case HollowShape.Triangle: | ||
470 | |||
471 | hollowVolume *= (0.5f * .5f); | ||
472 | break; | ||
473 | |||
474 | default: | ||
475 | hollowVolume = 0; | ||
476 | break; | ||
477 | } | ||
478 | volume *= (1.0f - hollowVolume); | ||
479 | } | ||
480 | } | ||
481 | |||
482 | else if (_pbs.PathCurve == (byte)Extrusion.Curve1) | ||
483 | { | ||
484 | //a tube | ||
485 | |||
486 | volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); | ||
487 | tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY); | ||
488 | volume -= volume*tmp*tmp; | ||
489 | |||
490 | if (hollowAmount > 0.0) | ||
491 | { | ||
492 | hollowVolume *= hollowAmount; | ||
493 | |||
494 | switch (_pbs.HollowShape) | ||
495 | { | ||
496 | case HollowShape.Square: | ||
497 | case HollowShape.Same: | ||
498 | break; | ||
499 | |||
500 | case HollowShape.Circle: | ||
501 | hollowVolume *= 0.78539816339f;; | ||
502 | break; | ||
503 | |||
504 | case HollowShape.Triangle: | ||
505 | hollowVolume *= 0.5f * 0.5f; | ||
506 | break; | ||
507 | default: | ||
508 | hollowVolume = 0; | ||
509 | break; | ||
510 | } | ||
511 | volume *= (1.0f - hollowVolume); | ||
512 | } | ||
513 | } | ||
514 | |||
515 | break; | ||
516 | |||
517 | case ProfileShape.Circle: | ||
518 | |||
519 | if (_pbs.PathCurve == (byte)Extrusion.Straight) | ||
520 | { | ||
521 | volume *= 0.78539816339f; // elipse base | ||
522 | |||
523 | if (hollowAmount > 0.0) | ||
524 | { | ||
525 | switch (_pbs.HollowShape) | ||
526 | { | ||
527 | case HollowShape.Same: | ||
528 | case HollowShape.Circle: | ||
529 | break; | ||
530 | |||
531 | case HollowShape.Square: | ||
532 | hollowVolume *= 0.5f * 2.5984480504799f; | ||
533 | break; | ||
534 | |||
535 | case HollowShape.Triangle: | ||
536 | hollowVolume *= .5f * 1.27323954473516f; | ||
537 | break; | ||
538 | |||
539 | default: | ||
540 | hollowVolume = 0; | ||
541 | break; | ||
542 | } | ||
543 | volume *= (1.0f - hollowVolume); | ||
544 | } | ||
545 | } | ||
546 | |||
547 | else if (_pbs.PathCurve == (byte)Extrusion.Curve1) | ||
548 | { | ||
549 | volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); | ||
550 | tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); | ||
551 | volume *= (1.0f - tmp * tmp); | ||
552 | |||
553 | if (hollowAmount > 0.0) | ||
554 | { | ||
555 | |||
556 | // calculate the hollow volume by it's shape compared to the prim shape | ||
557 | hollowVolume *= hollowAmount; | ||
558 | |||
559 | switch (_pbs.HollowShape) | ||
560 | { | ||
561 | case HollowShape.Same: | ||
562 | case HollowShape.Circle: | ||
563 | break; | ||
564 | |||
565 | case HollowShape.Square: | ||
566 | hollowVolume *= 0.5f * 2.5984480504799f; | ||
567 | break; | ||
568 | |||
569 | case HollowShape.Triangle: | ||
570 | hollowVolume *= .5f * 1.27323954473516f; | ||
571 | break; | ||
572 | |||
573 | default: | ||
574 | hollowVolume = 0; | ||
575 | break; | ||
576 | } | ||
577 | volume *= (1.0f - hollowVolume); | ||
578 | } | ||
579 | } | ||
580 | break; | ||
581 | 466 | ||
582 | case ProfileShape.HalfCircle: | 467 | public override Vector3 CenterOfMass |
583 | if (_pbs.PathCurve == (byte)Extrusion.Curve1) | 468 | { |
469 | get | ||
470 | { | ||
471 | lock (_parent_scene.OdeLock) | ||
472 | { | ||
473 | d.Vector3 dtmp; | ||
474 | if (!childPrim && Body != IntPtr.Zero) | ||
584 | { | 475 | { |
585 | volume *= 0.52359877559829887307710723054658f; | 476 | dtmp = d.BodyGetPosition(Body); |
477 | return new Vector3(dtmp.X, dtmp.Y, dtmp.Z); | ||
586 | } | 478 | } |
587 | break; | 479 | else if (prim_geom != IntPtr.Zero) |
480 | { | ||
481 | d.Quaternion dq; | ||
482 | d.GeomCopyQuaternion(prim_geom, out dq); | ||
483 | Quaternion q; | ||
484 | q.X = dq.X; | ||
485 | q.Y = dq.Y; | ||
486 | q.Z = dq.Z; | ||
487 | q.W = dq.W; | ||
488 | |||
489 | Vector3 Ptot = m_OBBOffset * q; | ||
490 | dtmp = d.GeomGetPosition(prim_geom); | ||
491 | Ptot.X += dtmp.X; | ||
492 | Ptot.Y += dtmp.Y; | ||
493 | Ptot.Z += dtmp.Z; | ||
494 | |||
495 | // if(childPrim) we only know about physical linksets | ||
496 | return Ptot; | ||
497 | /* | ||
498 | float tmass = _mass; | ||
499 | Ptot *= tmass; | ||
588 | 500 | ||
589 | case ProfileShape.EquilateralTriangle: | 501 | float m; |
590 | 502 | ||
591 | if (_pbs.PathCurve == (byte)Extrusion.Straight) | 503 | foreach (OdePrim prm in childrenPrim) |
592 | { | 504 | { |
593 | volume *= 0.32475953f; | 505 | m = prm._mass; |
594 | 506 | Ptot += prm.CenterOfMass * m; | |
595 | if (hollowAmount > 0.0) | 507 | tmass += m; |
596 | { | ||
597 | |||
598 | // calculate the hollow volume by it's shape compared to the prim shape | ||
599 | switch (_pbs.HollowShape) | ||
600 | { | ||
601 | case HollowShape.Same: | ||
602 | case HollowShape.Triangle: | ||
603 | hollowVolume *= .25f; | ||
604 | break; | ||
605 | |||
606 | case HollowShape.Square: | ||
607 | hollowVolume *= 0.499849f * 3.07920140172638f; | ||
608 | break; | ||
609 | |||
610 | case HollowShape.Circle: | ||
611 | // Hollow shape is a perfect cyllinder in respect to the cube's scale | ||
612 | // Cyllinder hollow volume calculation | ||
613 | |||
614 | hollowVolume *= 0.1963495f * 3.07920140172638f; | ||
615 | break; | ||
616 | |||
617 | default: | ||
618 | hollowVolume = 0; | ||
619 | break; | ||
620 | } | ||
621 | volume *= (1.0f - hollowVolume); | ||
622 | } | ||
623 | } | 508 | } |
624 | else if (_pbs.PathCurve == (byte)Extrusion.Curve1) | ||
625 | { | ||
626 | volume *= 0.32475953f; | ||
627 | volume *= 0.01f * (float)(200 - _pbs.PathScaleX); | ||
628 | tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); | ||
629 | volume *= (1.0f - tmp * tmp); | ||
630 | |||
631 | if (hollowAmount > 0.0) | ||
632 | { | ||
633 | |||
634 | hollowVolume *= hollowAmount; | ||
635 | 509 | ||
636 | switch (_pbs.HollowShape) | 510 | if (tmass == 0) |
637 | { | 511 | tmass = 0; |
638 | case HollowShape.Same: | 512 | else |
639 | case HollowShape.Triangle: | 513 | tmass = 1.0f / tmass; |
640 | hollowVolume *= .25f; | ||
641 | break; | ||
642 | |||
643 | case HollowShape.Square: | ||
644 | hollowVolume *= 0.499849f * 3.07920140172638f; | ||
645 | break; | ||
646 | |||
647 | case HollowShape.Circle: | ||
648 | 514 | ||
649 | hollowVolume *= 0.1963495f * 3.07920140172638f; | 515 | Ptot *= tmass; |
650 | break; | 516 | return Ptot; |
517 | */ | ||
518 | } | ||
519 | else | ||
520 | return _position; | ||
521 | } | ||
522 | } | ||
523 | } | ||
651 | 524 | ||
652 | default: | 525 | public override Vector3 OOBsize |
653 | hollowVolume = 0; | 526 | { |
654 | break; | 527 | get |
655 | } | 528 | { |
656 | volume *= (1.0f - hollowVolume); | 529 | return m_OBB; |
657 | } | 530 | } |
658 | } | 531 | } |
659 | break; | ||
660 | 532 | ||
661 | default: | 533 | public override Vector3 OOBoffset |
662 | break; | 534 | { |
535 | get | ||
536 | { | ||
537 | return m_OBBOffset; | ||
663 | } | 538 | } |
539 | } | ||
664 | 540 | ||
665 | float taperX1; | 541 | public override float OOBRadiusSQ |
666 | float taperY1; | 542 | { |
667 | float taperX; | 543 | get |
668 | float taperY; | 544 | { |
669 | float pathBegin; | 545 | return primOOBradiusSQ; |
670 | float pathEnd; | 546 | } |
671 | float profileBegin; | 547 | } |
672 | float profileEnd; | ||
673 | 548 | ||
674 | if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) | 549 | public override PrimitiveBaseShape Shape |
550 | { | ||
551 | set | ||
675 | { | 552 | { |
676 | taperX1 = _pbs.PathScaleX * 0.01f; | 553 | // AddChange(changes.Shape, value); |
677 | if (taperX1 > 1.0f) | 554 | _parent_scene.m_meshWorker.ChangeActorPhysRep(this, value, _size, m_shapetype); |
678 | taperX1 = 2.0f - taperX1; | 555 | } |
679 | taperX = 1.0f - taperX1; | 556 | } |
680 | 557 | ||
681 | taperY1 = _pbs.PathScaleY * 0.01f; | 558 | public override byte PhysicsShapeType |
682 | if (taperY1 > 1.0f) | 559 | { |
683 | taperY1 = 2.0f - taperY1; | 560 | get |
684 | taperY = 1.0f - taperY1; | 561 | { |
562 | return m_shapetype; | ||
685 | } | 563 | } |
686 | else | 564 | set |
687 | { | 565 | { |
688 | taperX = _pbs.PathTaperX * 0.01f; | 566 | m_shapetype = value; |
689 | if (taperX < 0.0f) | 567 | _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, _size, value); |
690 | taperX = -taperX; | ||
691 | taperX1 = 1.0f - taperX; | ||
692 | |||
693 | taperY = _pbs.PathTaperY * 0.01f; | ||
694 | if (taperY < 0.0f) | ||
695 | taperY = -taperY; | ||
696 | taperY1 = 1.0f - taperY; | ||
697 | } | 568 | } |
569 | } | ||
698 | 570 | ||
699 | volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); | 571 | public override Vector3 Velocity |
700 | 572 | { | |
701 | pathBegin = (float)_pbs.PathBegin * 2.0e-5f; | 573 | get |
702 | pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; | 574 | { |
703 | volume *= (pathEnd - pathBegin); | 575 | if (_zeroFlag) |
704 | 576 | return Vector3.Zero; | |
705 | // this is crude aproximation | 577 | return _velocity; |
706 | profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; | 578 | } |
707 | profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; | 579 | set |
708 | volume *= (profileEnd - profileBegin); | ||
709 | |||
710 | returnMass = m_density * volume; | ||
711 | |||
712 | if (returnMass <= 0) | ||
713 | returnMass = 0.0001f;//ckrinke: Mass must be greater then zero. | ||
714 | // else if (returnMass > _parent_scene.maximumMassObject) | ||
715 | // returnMass = _parent_scene.maximumMassObject; | ||
716 | |||
717 | // Recursively calculate mass | ||
718 | bool HasChildPrim = false; | ||
719 | lock (childrenPrim) | ||
720 | { | 580 | { |
721 | if (childrenPrim.Count > 0) | 581 | if (value.IsFinite()) |
582 | { | ||
583 | AddChange(changes.Velocity, value); | ||
584 | } | ||
585 | else | ||
722 | { | 586 | { |
723 | HasChildPrim = true; | 587 | m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name); |
724 | } | 588 | } |
589 | |||
725 | } | 590 | } |
591 | } | ||
726 | 592 | ||
727 | if (HasChildPrim) | 593 | public override Vector3 Torque |
594 | { | ||
595 | get | ||
728 | { | 596 | { |
729 | OdePrim[] childPrimArr = new OdePrim[0]; | 597 | if (!IsPhysical || Body == IntPtr.Zero) |
598 | return Vector3.Zero; | ||
730 | 599 | ||
731 | lock (childrenPrim) | 600 | return _torque; |
732 | childPrimArr = childrenPrim.ToArray(); | 601 | } |
733 | 602 | ||
734 | for (int i = 0; i < childPrimArr.Length; i++) | 603 | set |
604 | { | ||
605 | if (value.IsFinite()) | ||
735 | { | 606 | { |
736 | if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove) | 607 | AddChange(changes.Torque, value); |
737 | returnMass += childPrimArr[i].CalculateMass(); | 608 | } |
738 | // failsafe, this shouldn't happen but with OpenSim, you never know :) | 609 | else |
739 | if (i > 256) | 610 | { |
740 | break; | 611 | m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name); |
741 | } | 612 | } |
742 | } | 613 | } |
614 | } | ||
743 | 615 | ||
744 | if (returnMass > _parent_scene.maximumMassObject) | 616 | public override float CollisionScore |
745 | returnMass = _parent_scene.maximumMassObject; | 617 | { |
746 | 618 | get { return m_collisionscore; } | |
747 | return returnMass; | 619 | set { m_collisionscore = value; } |
748 | } | 620 | } |
749 | 621 | ||
750 | #endregion | 622 | public override bool Kinematic |
623 | { | ||
624 | get { return false; } | ||
625 | set { } | ||
626 | } | ||
751 | 627 | ||
752 | private void setMass() | 628 | public override Quaternion Orientation |
753 | { | 629 | { |
754 | if (Body != (IntPtr) 0) | 630 | get |
755 | { | 631 | { |
756 | float newmass = CalculateMass(); | 632 | if (givefakeori > 0) |
757 | 633 | return fakeori; | |
758 | //m_log.Info("[PHYSICS]: New Mass: " + newmass.ToString()); | 634 | else |
759 | 635 | ||
760 | d.MassSetBoxTotal(out pMass, newmass, _size.X, _size.Y, _size.Z); | 636 | return _orientation; |
761 | d.BodySetMass(Body, ref pMass); | ||
762 | } | 637 | } |
763 | } | 638 | set |
764 | |||
765 | /// <summary> | ||
766 | /// Stop a prim from being subject to physics. | ||
767 | /// </summary> | ||
768 | internal void disableBody() | ||
769 | { | ||
770 | //this kills the body so things like 'mesh' can re-create it. | ||
771 | lock (this) | ||
772 | { | 639 | { |
773 | if (!childPrim) | 640 | if (QuaternionIsFinite(value)) |
774 | { | 641 | { |
775 | if (Body != IntPtr.Zero) | 642 | fakeori = value; |
776 | { | 643 | givefakeori++; |
777 | _parent_scene.DeactivatePrim(this); | ||
778 | m_collisionCategories &= ~CollisionCategories.Body; | ||
779 | m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); | ||
780 | 644 | ||
781 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | 645 | value.Normalize(); |
782 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
783 | 646 | ||
784 | d.BodyDestroy(Body); | 647 | AddChange(changes.Orientation, value); |
785 | lock (childrenPrim) | ||
786 | { | ||
787 | if (childrenPrim.Count > 0) | ||
788 | { | ||
789 | foreach (OdePrim prm in childrenPrim) | ||
790 | { | ||
791 | _parent_scene.DeactivatePrim(prm); | ||
792 | prm.Body = IntPtr.Zero; | ||
793 | } | ||
794 | } | ||
795 | } | ||
796 | Body = IntPtr.Zero; | ||
797 | } | ||
798 | } | 648 | } |
799 | else | 649 | else |
800 | { | 650 | m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name); |
801 | _parent_scene.DeactivatePrim(this); | ||
802 | |||
803 | m_collisionCategories &= ~CollisionCategories.Body; | ||
804 | m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); | ||
805 | |||
806 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | ||
807 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
808 | 651 | ||
809 | Body = IntPtr.Zero; | ||
810 | } | ||
811 | } | 652 | } |
812 | |||
813 | m_disabled = true; | ||
814 | m_collisionscore = 0; | ||
815 | } | 653 | } |
816 | 654 | ||
817 | private static Dictionary<IMesh, IntPtr> m_MeshToTriMeshMap = new Dictionary<IMesh, IntPtr>(); | 655 | public override Vector3 Acceleration |
818 | |||
819 | private void setMesh(OdeScene parent_scene, IMesh mesh) | ||
820 | { | 656 | { |
821 | // m_log.DebugFormat("[ODE PRIM]: Setting mesh on {0} to {1}", Name, mesh); | 657 | get { return _acceleration; } |
658 | set { } | ||
659 | } | ||
822 | 660 | ||
823 | // This sleeper is there to moderate how long it takes between | 661 | public override Vector3 RotationalVelocity |
824 | // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object | 662 | { |
663 | get | ||
664 | { | ||
665 | Vector3 pv = Vector3.Zero; | ||
666 | if (_zeroFlag) | ||
667 | return pv; | ||
825 | 668 | ||
826 | //Thread.Sleep(10); | 669 | if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) |
670 | return pv; | ||
827 | 671 | ||
828 | //Kill Body so that mesh can re-make the geom | 672 | return m_rotationalVelocity; |
829 | if (IsPhysical && Body != IntPtr.Zero) | 673 | } |
674 | set | ||
830 | { | 675 | { |
831 | if (childPrim) | 676 | if (value.IsFinite()) |
832 | { | 677 | { |
833 | if (_parent != null) | 678 | AddChange(changes.AngVelocity, value); |
834 | { | ||
835 | OdePrim parent = (OdePrim)_parent; | ||
836 | parent.ChildDelink(this); | ||
837 | } | ||
838 | } | 679 | } |
839 | else | 680 | else |
840 | { | 681 | { |
841 | disableBody(); | 682 | m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name); |
842 | } | 683 | } |
843 | } | 684 | } |
685 | } | ||
844 | 686 | ||
845 | IntPtr vertices, indices; | 687 | public override float Buoyancy |
846 | int vertexCount, indexCount; | 688 | { |
847 | int vertexStride, triStride; | 689 | get { return m_buoyancy; } |
848 | mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap | 690 | set |
849 | mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage | 691 | { |
850 | m_expectedCollisionContacts = indexCount; | 692 | AddChange(changes.Buoyancy,value); |
851 | mesh.releaseSourceMeshData(); // free up the original mesh data to save memory | 693 | } |
694 | } | ||
695 | |||
696 | public override bool FloatOnWater | ||
697 | { | ||
698 | set | ||
699 | { | ||
700 | AddChange(changes.CollidesWater, value); | ||
701 | } | ||
702 | } | ||
852 | 703 | ||
853 | // We must lock here since m_MeshToTriMeshMap is static and multiple scene threads may call this method at | 704 | public override Vector3 PIDTarget |
854 | // the same time. | 705 | { |
855 | lock (m_MeshToTriMeshMap) | 706 | set |
856 | { | 707 | { |
857 | if (m_MeshToTriMeshMap.ContainsKey(mesh)) | 708 | if (value.IsFinite()) |
858 | { | 709 | { |
859 | _triMeshData = m_MeshToTriMeshMap[mesh]; | 710 | AddChange(changes.PIDTarget,value); |
860 | } | 711 | } |
861 | else | 712 | else |
713 | m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); | ||
714 | } | ||
715 | } | ||
716 | |||
717 | public override bool PIDActive | ||
718 | { | ||
719 | set | ||
720 | { | ||
721 | AddChange(changes.PIDActive,value); | ||
722 | } | ||
723 | } | ||
724 | |||
725 | public override float PIDTau | ||
726 | { | ||
727 | set | ||
728 | { | ||
729 | float tmp = 0; | ||
730 | if (value > 0) | ||
862 | { | 731 | { |
863 | _triMeshData = d.GeomTriMeshDataCreate(); | 732 | float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep); |
864 | 733 | if (value < mint) | |
865 | d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); | 734 | tmp = mint; |
866 | d.GeomTriMeshDataPreprocess(_triMeshData); | 735 | else |
867 | m_MeshToTriMeshMap[mesh] = _triMeshData; | 736 | tmp = value; |
868 | } | 737 | } |
738 | AddChange(changes.PIDTau,tmp); | ||
869 | } | 739 | } |
740 | } | ||
870 | 741 | ||
871 | // _parent_scene.waitForSpaceUnlock(m_targetSpace); | 742 | public override float PIDHoverHeight |
872 | try | 743 | { |
744 | set | ||
873 | { | 745 | { |
874 | SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null)); | 746 | AddChange(changes.PIDHoverHeight,value); |
875 | } | 747 | } |
876 | catch (AccessViolationException) | 748 | } |
749 | public override bool PIDHoverActive | ||
750 | { | ||
751 | set | ||
877 | { | 752 | { |
878 | m_log.ErrorFormat("[PHYSICS]: MESH LOCKED FOR {0}", Name); | 753 | AddChange(changes.PIDHoverActive, value); |
879 | return; | ||
880 | } | 754 | } |
755 | } | ||
881 | 756 | ||
882 | // if (IsPhysical && Body == (IntPtr) 0) | 757 | public override PIDHoverType PIDHoverType |
883 | // { | 758 | { |
884 | // Recreate the body | 759 | set |
885 | // m_interpenetrationcount = 0; | 760 | { |
886 | // m_collisionscore = 0; | 761 | AddChange(changes.PIDHoverType,value); |
887 | 762 | } | |
888 | // enableBody(); | ||
889 | // } | ||
890 | } | 763 | } |
891 | 764 | ||
892 | internal void ProcessTaints() | 765 | public override float PIDHoverTau |
893 | { | 766 | { |
894 | #if SPAM | 767 | set |
895 | Console.WriteLine("ZProcessTaints for " + Name); | 768 | { |
896 | #endif | 769 | float tmp =0; |
770 | if (value > 0) | ||
771 | { | ||
772 | float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep); | ||
773 | if (value < mint) | ||
774 | tmp = mint; | ||
775 | else | ||
776 | tmp = value; | ||
777 | } | ||
778 | AddChange(changes.PIDHoverTau, tmp); | ||
779 | } | ||
780 | } | ||
897 | 781 | ||
898 | // This must be processed as the very first taint so that later operations have a prim_geom to work with | 782 | public override Quaternion APIDTarget { set { return; } } |
899 | // if this is a new prim. | ||
900 | if (m_taintadd) | ||
901 | changeadd(); | ||
902 | 783 | ||
903 | if (!_position.ApproxEquals(m_taintposition, 0f)) | 784 | public override bool APIDActive { set { return; } } |
904 | changemove(); | ||
905 | 785 | ||
906 | if (m_taintrot != _orientation) | 786 | public override float APIDStrength { set { return; } } |
787 | |||
788 | public override float APIDDamping { set { return; } } | ||
789 | |||
790 | public override int VehicleType | ||
791 | { | ||
792 | // we may need to put a fake on this | ||
793 | get | ||
907 | { | 794 | { |
908 | if (childPrim && IsPhysical) // For physical child prim... | 795 | if (m_vehicle == null) |
909 | { | 796 | return (int)Vehicle.TYPE_NONE; |
910 | rotate(); | ||
911 | // KF: ODE will also rotate the parent prim! | ||
912 | // so rotate the root back to where it was | ||
913 | OdePrim parent = (OdePrim)_parent; | ||
914 | parent.rotate(); | ||
915 | } | ||
916 | else | 797 | else |
917 | { | 798 | return (int)m_vehicle.Type; |
918 | //Just rotate the prim | ||
919 | rotate(); | ||
920 | } | ||
921 | } | 799 | } |
922 | 800 | set | |
923 | if (m_taintPhysics != IsPhysical && !(m_taintparent != _parent)) | 801 | { |
924 | changePhysicsStatus(); | 802 | AddChange(changes.VehicleType, value); |
803 | } | ||
804 | } | ||
805 | |||
806 | public override void VehicleFloatParam(int param, float value) | ||
807 | { | ||
808 | strVehicleFloatParam fp = new strVehicleFloatParam(); | ||
809 | fp.param = param; | ||
810 | fp.value = value; | ||
811 | AddChange(changes.VehicleFloatParam, fp); | ||
812 | } | ||
813 | |||
814 | public override void VehicleVectorParam(int param, Vector3 value) | ||
815 | { | ||
816 | strVehicleVectorParam fp = new strVehicleVectorParam(); | ||
817 | fp.param = param; | ||
818 | fp.value = value; | ||
819 | AddChange(changes.VehicleVectorParam, fp); | ||
820 | } | ||
821 | |||
822 | public override void VehicleRotationParam(int param, Quaternion value) | ||
823 | { | ||
824 | strVehicleQuatParam fp = new strVehicleQuatParam(); | ||
825 | fp.param = param; | ||
826 | fp.value = value; | ||
827 | AddChange(changes.VehicleRotationParam, fp); | ||
828 | } | ||
829 | |||
830 | public override void VehicleFlags(int param, bool value) | ||
831 | { | ||
832 | strVehicleBoolParam bp = new strVehicleBoolParam(); | ||
833 | bp.param = param; | ||
834 | bp.value = value; | ||
835 | AddChange(changes.VehicleFlags, bp); | ||
836 | } | ||
837 | |||
838 | public override void SetVehicle(object vdata) | ||
839 | { | ||
840 | AddChange(changes.SetVehicle, vdata); | ||
841 | } | ||
842 | public void SetAcceleration(Vector3 accel) | ||
843 | { | ||
844 | _acceleration = accel; | ||
845 | } | ||
846 | |||
847 | public override void AddForce(Vector3 force, bool pushforce) | ||
848 | { | ||
849 | if (force.IsFinite()) | ||
850 | { | ||
851 | if(pushforce) | ||
852 | AddChange(changes.AddForce, force); | ||
853 | else // a impulse | ||
854 | AddChange(changes.AddForce, force * m_invTimeStep); | ||
855 | } | ||
856 | else | ||
857 | { | ||
858 | m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name); | ||
859 | } | ||
860 | //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); | ||
861 | } | ||
925 | 862 | ||
926 | if (!_size.ApproxEquals(m_taintsize, 0f)) | 863 | public override void AddAngularForce(Vector3 force, bool pushforce) |
927 | changesize(); | 864 | { |
865 | if (force.IsFinite()) | ||
866 | { | ||
867 | // if(pushforce) for now applyrotationimpulse seems more happy applied as a force | ||
868 | AddChange(changes.AddAngForce, force); | ||
869 | // else // a impulse | ||
870 | // AddChange(changes.AddAngForce, force * m_invTimeStep); | ||
871 | } | ||
872 | else | ||
873 | { | ||
874 | m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name); | ||
875 | } | ||
876 | } | ||
928 | 877 | ||
929 | if (m_taintshape) | 878 | public override void CrossingFailure() |
930 | changeshape(); | 879 | { |
880 | if (m_outbounds) | ||
881 | { | ||
882 | _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); | ||
883 | _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); | ||
884 | _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f); | ||
931 | 885 | ||
932 | if (m_taintforce) | 886 | m_lastposition = _position; |
933 | changeAddForce(); | 887 | _velocity.X = 0; |
888 | _velocity.Y = 0; | ||
889 | _velocity.Z = 0; | ||
934 | 890 | ||
935 | if (m_taintaddangularforce) | 891 | m_lastVelocity = _velocity; |
936 | changeAddAngularForce(); | 892 | if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) |
893 | m_vehicle.Stop(); | ||
937 | 894 | ||
938 | if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f)) | 895 | if(Body != IntPtr.Zero) |
939 | changeSetTorque(); | 896 | d.BodySetLinearVel(Body, 0, 0, 0); // stop it |
897 | if (prim_geom != IntPtr.Zero) | ||
898 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); | ||
940 | 899 | ||
941 | if (m_taintdisable) | 900 | m_outbounds = false; |
942 | changedisable(); | 901 | changeDisable(false); |
902 | base.RequestPhysicsterseUpdate(); | ||
903 | } | ||
904 | } | ||
943 | 905 | ||
944 | if (m_taintselected != m_isSelected) | 906 | public override void SetMomentum(Vector3 momentum) |
945 | changeSelectedStatus(); | 907 | { |
908 | } | ||
946 | 909 | ||
947 | if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f)) | 910 | public override void SetMaterial(int pMaterial) |
948 | changevelocity(); | 911 | { |
912 | m_material = pMaterial; | ||
913 | mu = _parent_scene.m_materialContactsData[pMaterial].mu; | ||
914 | bounce = _parent_scene.m_materialContactsData[pMaterial].bounce; | ||
915 | } | ||
949 | 916 | ||
950 | if (m_taintparent != _parent) | 917 | public void setPrimForRemoval() |
951 | changelink(); | 918 | { |
919 | AddChange(changes.Remove, null); | ||
920 | } | ||
952 | 921 | ||
953 | if (m_taintCollidesWater != m_collidesWater) | 922 | public override void link(PhysicsActor obj) |
954 | changefloatonwater(); | 923 | { |
924 | AddChange(changes.Link, obj); | ||
925 | } | ||
955 | 926 | ||
956 | if (!m_angularlock.ApproxEquals(m_taintAngularLock,0f)) | 927 | public override void delink() |
957 | changeAngularLock(); | 928 | { |
929 | AddChange(changes.DeLink, null); | ||
958 | } | 930 | } |
959 | 931 | ||
960 | /// <summary> | 932 | public override void LockAngularMotion(Vector3 axis) |
961 | /// Change prim in response to an angular lock taint. | ||
962 | /// </summary> | ||
963 | private void changeAngularLock() | ||
964 | { | 933 | { |
965 | // do we have a Physical object? | 934 | // reverse the zero/non zero values for ODE. |
966 | if (Body != IntPtr.Zero) | 935 | if (axis.IsFinite()) |
967 | { | 936 | { |
968 | //Check that we have a Parent | 937 | axis.X = (axis.X > 0) ? 1f : 0f; |
969 | //If we have a parent then we're not authorative here | 938 | axis.Y = (axis.Y > 0) ? 1f : 0f; |
970 | if (_parent == null) | 939 | axis.Z = (axis.Z > 0) ? 1f : 0f; |
971 | { | 940 | // m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); |
972 | if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f)) | 941 | AddChange(changes.AngLock, axis); |
973 | { | ||
974 | //d.BodySetFiniteRotationMode(Body, 0); | ||
975 | //d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z); | ||
976 | createAMotor(m_taintAngularLock); | ||
977 | } | ||
978 | else | ||
979 | { | ||
980 | if (Amotor != IntPtr.Zero) | ||
981 | { | ||
982 | d.JointDestroy(Amotor); | ||
983 | Amotor = IntPtr.Zero; | ||
984 | } | ||
985 | } | ||
986 | } | ||
987 | } | 942 | } |
943 | else | ||
944 | { | ||
945 | m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name); | ||
946 | } | ||
947 | } | ||
988 | 948 | ||
989 | // Store this for later in case we get turned into a separate body | 949 | public override void SubscribeEvents(int ms) |
990 | m_angularlock = m_taintAngularLock; | 950 | { |
951 | m_eventsubscription = ms; | ||
952 | m_cureventsubscription = 0; | ||
953 | if (CollisionEventsThisFrame == null) | ||
954 | CollisionEventsThisFrame = new CollisionEventUpdate(); | ||
955 | SentEmptyCollisionsEvent = false; | ||
991 | } | 956 | } |
992 | 957 | ||
993 | /// <summary> | 958 | public override void UnSubscribeEvents() |
994 | /// Change prim in response to a link taint. | ||
995 | /// </summary> | ||
996 | private void changelink() | ||
997 | { | 959 | { |
998 | // If the newly set parent is not null | 960 | if (CollisionEventsThisFrame != null) |
999 | // create link | ||
1000 | if (_parent == null && m_taintparent != null) | ||
1001 | { | 961 | { |
1002 | if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim) | 962 | CollisionEventsThisFrame.Clear(); |
1003 | { | 963 | CollisionEventsThisFrame = null; |
1004 | OdePrim obj = (OdePrim)m_taintparent; | ||
1005 | //obj.disableBody(); | ||
1006 | //Console.WriteLine("changelink calls ParentPrim"); | ||
1007 | obj.AddChildPrim(this); | ||
1008 | |||
1009 | /* | ||
1010 | if (obj.Body != (IntPtr)0 && Body != (IntPtr)0 && obj.Body != Body) | ||
1011 | { | ||
1012 | _linkJointGroup = d.JointGroupCreate(0); | ||
1013 | m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); | ||
1014 | d.JointAttach(m_linkJoint, obj.Body, Body); | ||
1015 | d.JointSetFixed(m_linkJoint); | ||
1016 | } | ||
1017 | */ | ||
1018 | } | ||
1019 | } | 964 | } |
1020 | // If the newly set parent is null | 965 | m_eventsubscription = 0; |
1021 | // destroy link | 966 | _parent_scene.RemoveCollisionEventReporting(this); |
1022 | else if (_parent != null && m_taintparent == null) | 967 | } |
968 | |||
969 | public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact) | ||
970 | { | ||
971 | if (CollisionEventsThisFrame == null) | ||
972 | CollisionEventsThisFrame = new CollisionEventUpdate(); | ||
973 | // if(CollisionEventsThisFrame.Count < 32) | ||
974 | CollisionEventsThisFrame.AddCollider(CollidedWith, contact); | ||
975 | } | ||
976 | |||
977 | public void SendCollisions() | ||
978 | { | ||
979 | if (CollisionEventsThisFrame == null) | ||
980 | return; | ||
981 | |||
982 | if (m_cureventsubscription < m_eventsubscription) | ||
983 | return; | ||
984 | |||
985 | m_cureventsubscription = 0; | ||
986 | |||
987 | int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count; | ||
988 | |||
989 | if (!SentEmptyCollisionsEvent || ncolisions > 0) | ||
1023 | { | 990 | { |
1024 | //Console.WriteLine(" changelink B"); | 991 | base.SendCollisionUpdate(CollisionEventsThisFrame); |
1025 | 992 | ||
1026 | if (_parent is OdePrim) | 993 | if (ncolisions == 0) |
1027 | { | 994 | { |
1028 | OdePrim obj = (OdePrim)_parent; | 995 | SentEmptyCollisionsEvent = true; |
1029 | obj.ChildDelink(this); | 996 | _parent_scene.RemoveCollisionEventReporting(this); |
1030 | childPrim = false; | ||
1031 | //_parent = null; | ||
1032 | } | 997 | } |
1033 | 998 | else | |
1034 | /* | 999 | { |
1035 | if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0) | 1000 | SentEmptyCollisionsEvent = false; |
1036 | d.JointGroupDestroy(_linkJointGroup); | 1001 | CollisionEventsThisFrame.Clear(); |
1037 | 1002 | } | |
1038 | _linkJointGroup = (IntPtr)0; | 1003 | } |
1039 | m_linkJoint = (IntPtr)0; | ||
1040 | */ | ||
1041 | } | ||
1042 | |||
1043 | _parent = m_taintparent; | ||
1044 | m_taintPhysics = IsPhysical; | ||
1045 | } | 1004 | } |
1046 | 1005 | ||
1047 | /// <summary> | 1006 | internal void AddCollisionFrameTime(int t) |
1048 | /// Add a child prim to this parent prim. | ||
1049 | /// </summary> | ||
1050 | /// <param name="prim">Child prim</param> | ||
1051 | private void AddChildPrim(OdePrim prim) | ||
1052 | { | 1007 | { |
1053 | if (LocalID == prim.LocalID) | 1008 | if (m_cureventsubscription < 50000) |
1054 | return; | 1009 | m_cureventsubscription += t; |
1010 | } | ||
1055 | 1011 | ||
1056 | if (Body == IntPtr.Zero) | 1012 | public override bool SubscribedEvents() |
1013 | { | ||
1014 | if (m_eventsubscription > 0) | ||
1015 | return true; | ||
1016 | return false; | ||
1017 | } | ||
1018 | |||
1019 | public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, | ||
1020 | Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID) | ||
1021 | { | ||
1022 | Name = primName; | ||
1023 | LocalID = plocalID; | ||
1024 | |||
1025 | m_vehicle = null; | ||
1026 | |||
1027 | if (!pos.IsFinite()) | ||
1057 | { | 1028 | { |
1058 | Body = d.BodyCreate(_parent_scene.world); | 1029 | pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), |
1059 | setMass(); | 1030 | parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); |
1031 | m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name); | ||
1060 | } | 1032 | } |
1033 | _position = pos; | ||
1034 | givefakepos = 0; | ||
1061 | 1035 | ||
1062 | lock (childrenPrim) | 1036 | m_timeStep = parent_scene.ODE_STEPSIZE; |
1037 | m_invTimeStep = 1f / m_timeStep; | ||
1038 | |||
1039 | m_density = parent_scene.geomDefaultDensity; | ||
1040 | body_autodisable_frames = parent_scene.bodyFramesAutoDisable; | ||
1041 | |||
1042 | prim_geom = IntPtr.Zero; | ||
1043 | collide_geom = IntPtr.Zero; | ||
1044 | Body = IntPtr.Zero; | ||
1045 | |||
1046 | if (!size.IsFinite()) | ||
1063 | { | 1047 | { |
1064 | if (childrenPrim.Contains(prim)) | 1048 | size = new Vector3(0.5f, 0.5f, 0.5f); |
1065 | return; | 1049 | m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name); |
1050 | } | ||
1066 | 1051 | ||
1067 | // m_log.DebugFormat( | 1052 | if (size.X <= 0) size.X = 0.01f; |
1068 | // "[ODE PRIM]: Linking prim {0} {1} to {2} {3}", prim.Name, prim.LocalID, Name, LocalID); | 1053 | if (size.Y <= 0) size.Y = 0.01f; |
1054 | if (size.Z <= 0) size.Z = 0.01f; | ||
1069 | 1055 | ||
1070 | childrenPrim.Add(prim); | 1056 | _size = size; |
1071 | 1057 | ||
1072 | foreach (OdePrim prm in childrenPrim) | 1058 | if (!QuaternionIsFinite(rotation)) |
1073 | { | 1059 | { |
1074 | d.Mass m2; | 1060 | rotation = Quaternion.Identity; |
1075 | d.MassSetZero(out m2); | 1061 | m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name); |
1076 | d.MassSetBoxTotal(out m2, prim.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z); | 1062 | } |
1077 | 1063 | ||
1078 | d.Quaternion quat = new d.Quaternion(); | 1064 | _orientation = rotation; |
1079 | quat.W = prm._orientation.W; | 1065 | givefakeori = 0; |
1080 | quat.X = prm._orientation.X; | ||
1081 | quat.Y = prm._orientation.Y; | ||
1082 | quat.Z = prm._orientation.Z; | ||
1083 | 1066 | ||
1084 | d.Matrix3 mat = new d.Matrix3(); | 1067 | _pbs = pbs; |
1085 | d.RfromQ(out mat, ref quat); | ||
1086 | d.MassRotate(ref m2, ref mat); | ||
1087 | d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z); | ||
1088 | d.MassAdd(ref pMass, ref m2); | ||
1089 | } | ||
1090 | 1068 | ||
1091 | foreach (OdePrim prm in childrenPrim) | 1069 | _parent_scene = parent_scene; |
1092 | { | 1070 | m_targetSpace = IntPtr.Zero; |
1093 | prm.m_collisionCategories |= CollisionCategories.Body; | ||
1094 | prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); | ||
1095 | 1071 | ||
1096 | //Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + Name); | 1072 | if (pos.Z < 0) |
1097 | d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); | 1073 | { |
1098 | d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); | 1074 | m_isphysical = false; |
1075 | } | ||
1076 | else | ||
1077 | { | ||
1078 | m_isphysical = pisPhysical; | ||
1079 | } | ||
1080 | m_fakeisphysical = m_isphysical; | ||
1099 | 1081 | ||
1100 | d.Quaternion quat = new d.Quaternion(); | 1082 | m_isVolumeDetect = false; |
1101 | quat.W = prm._orientation.W; | 1083 | m_fakeisVolumeDetect = false; |
1102 | quat.X = prm._orientation.X; | ||
1103 | quat.Y = prm._orientation.Y; | ||
1104 | quat.Z = prm._orientation.Z; | ||
1105 | 1084 | ||
1106 | d.Matrix3 mat = new d.Matrix3(); | 1085 | m_force = Vector3.Zero; |
1107 | d.RfromQ(out mat, ref quat); | ||
1108 | if (Body != IntPtr.Zero) | ||
1109 | { | ||
1110 | d.GeomSetBody(prm.prim_geom, Body); | ||
1111 | prm.childPrim = true; | ||
1112 | d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z); | ||
1113 | //d.GeomSetOffsetPosition(prim.prim_geom, | ||
1114 | // (Position.X - prm.Position.X) - pMass.c.X, | ||
1115 | // (Position.Y - prm.Position.Y) - pMass.c.Y, | ||
1116 | // (Position.Z - prm.Position.Z) - pMass.c.Z); | ||
1117 | d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); | ||
1118 | //d.GeomSetOffsetRotation(prm.prim_geom, ref mat); | ||
1119 | d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z); | ||
1120 | d.BodySetMass(Body, ref pMass); | ||
1121 | } | ||
1122 | else | ||
1123 | { | ||
1124 | m_log.DebugFormat("[PHYSICS]: {0} ain't got no boooooooooddy, no body", Name); | ||
1125 | } | ||
1126 | 1086 | ||
1127 | prm.m_interpenetrationcount = 0; | 1087 | m_iscolliding = false; |
1128 | prm.m_collisionscore = 0; | 1088 | m_colliderfilter = 0; |
1129 | prm.m_disabled = false; | 1089 | m_NoColide = false; |
1130 | 1090 | ||
1131 | // The body doesn't already have a finite rotation mode set here | 1091 | _triMeshData = IntPtr.Zero; |
1132 | if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null) | ||
1133 | { | ||
1134 | prm.createAMotor(m_angularlock); | ||
1135 | } | ||
1136 | prm.Body = Body; | ||
1137 | _parent_scene.ActivatePrim(prm); | ||
1138 | } | ||
1139 | |||
1140 | m_collisionCategories |= CollisionCategories.Body; | ||
1141 | m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); | ||
1142 | |||
1143 | //Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + Name); | ||
1144 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | ||
1145 | //Console.WriteLine(" Post GeomSetCategoryBits 2"); | ||
1146 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
1147 | |||
1148 | d.Quaternion quat2 = new d.Quaternion(); | ||
1149 | quat2.W = _orientation.W; | ||
1150 | quat2.X = _orientation.X; | ||
1151 | quat2.Y = _orientation.Y; | ||
1152 | quat2.Z = _orientation.Z; | ||
1153 | |||
1154 | d.Matrix3 mat2 = new d.Matrix3(); | ||
1155 | d.RfromQ(out mat2, ref quat2); | ||
1156 | d.GeomSetBody(prim_geom, Body); | ||
1157 | d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z); | ||
1158 | //d.GeomSetOffsetPosition(prim.prim_geom, | ||
1159 | // (Position.X - prm.Position.X) - pMass.c.X, | ||
1160 | // (Position.Y - prm.Position.Y) - pMass.c.Y, | ||
1161 | // (Position.Z - prm.Position.Z) - pMass.c.Z); | ||
1162 | //d.GeomSetOffsetRotation(prim_geom, ref mat2); | ||
1163 | d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z); | ||
1164 | d.BodySetMass(Body, ref pMass); | ||
1165 | |||
1166 | d.BodySetAutoDisableFlag(Body, true); | ||
1167 | d.BodySetAutoDisableSteps(Body, body_autodisable_frames); | ||
1168 | |||
1169 | m_interpenetrationcount = 0; | ||
1170 | m_collisionscore = 0; | ||
1171 | m_disabled = false; | ||
1172 | 1092 | ||
1173 | // The body doesn't already have a finite rotation mode set here | 1093 | m_shapetype = _shapeType; |
1174 | if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null) | ||
1175 | { | ||
1176 | createAMotor(m_angularlock); | ||
1177 | } | ||
1178 | 1094 | ||
1179 | d.BodySetPosition(Body, Position.X, Position.Y, Position.Z); | 1095 | m_lastdoneSelected = false; |
1096 | m_isSelected = false; | ||
1097 | m_delaySelect = false; | ||
1180 | 1098 | ||
1181 | if (m_vehicle.Type != Vehicle.TYPE_NONE) | 1099 | m_isphantom = pisPhantom; |
1182 | m_vehicle.Enable(Body, _parent_scene); | 1100 | m_fakeisphantom = pisPhantom; |
1183 | 1101 | ||
1184 | _parent_scene.ActivatePrim(this); | 1102 | mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu; |
1185 | } | 1103 | bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce; |
1104 | |||
1105 | m_building = true; // control must set this to false when done | ||
1106 | |||
1107 | _parent_scene.m_meshWorker.NewActorPhysRep(this, _pbs, _size, m_shapetype); | ||
1186 | } | 1108 | } |
1187 | 1109 | ||
1188 | private void ChildSetGeom(OdePrim odePrim) | 1110 | private void resetCollisionAccounting() |
1189 | { | 1111 | { |
1190 | // m_log.DebugFormat( | 1112 | m_collisionscore = 0; |
1191 | // "[ODE PRIM]: ChildSetGeom {0} {1} for {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID); | 1113 | } |
1192 | 1114 | ||
1193 | //if (IsPhysical && Body != IntPtr.Zero) | 1115 | private void UpdateCollisionCatFlags() |
1194 | lock (childrenPrim) | 1116 | { |
1117 | if(m_isphysical && m_disabled) | ||
1195 | { | 1118 | { |
1196 | foreach (OdePrim prm in childrenPrim) | 1119 | m_collisionCategories = 0; |
1197 | { | 1120 | m_collisionFlags = 0; |
1198 | //prm.childPrim = true; | ||
1199 | prm.disableBody(); | ||
1200 | //prm.m_taintparent = null; | ||
1201 | //prm._parent = null; | ||
1202 | //prm.m_taintPhysics = false; | ||
1203 | //prm.m_disabled = true; | ||
1204 | //prm.childPrim = false; | ||
1205 | } | ||
1206 | } | 1121 | } |
1207 | 1122 | ||
1208 | disableBody(); | 1123 | else if (m_isSelected) |
1124 | { | ||
1125 | m_collisionCategories = CollisionCategories.Selected; | ||
1126 | m_collisionFlags = 0; | ||
1127 | } | ||
1209 | 1128 | ||
1210 | // Spurious - Body == IntPtr.Zero after disableBody() | 1129 | else if (m_isVolumeDetect) |
1211 | // if (Body != IntPtr.Zero) | 1130 | { |
1212 | // { | 1131 | m_collisionCategories = CollisionCategories.VolumeDtc; |
1213 | // _parent_scene.DeactivatePrim(this); | 1132 | if (m_isphysical) |
1214 | // } | 1133 | m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character; |
1134 | else | ||
1135 | m_collisionFlags = 0; | ||
1136 | } | ||
1137 | else if (m_isphantom) | ||
1138 | { | ||
1139 | m_collisionCategories = CollisionCategories.Phantom; | ||
1140 | if (m_isphysical) | ||
1141 | m_collisionFlags = CollisionCategories.Land; | ||
1142 | else | ||
1143 | m_collisionFlags = 0; | ||
1144 | } | ||
1145 | else | ||
1146 | { | ||
1147 | m_collisionCategories = CollisionCategories.Geom; | ||
1148 | if (m_isphysical) | ||
1149 | m_collisionFlags = m_default_collisionFlagsPhysical; | ||
1150 | else | ||
1151 | m_collisionFlags = m_default_collisionFlagsNotPhysical; | ||
1152 | } | ||
1153 | } | ||
1215 | 1154 | ||
1216 | lock (childrenPrim) | 1155 | private void ApplyCollisionCatFlags() |
1156 | { | ||
1157 | if (prim_geom != IntPtr.Zero) | ||
1217 | { | 1158 | { |
1218 | foreach (OdePrim prm in childrenPrim) | 1159 | if (!childPrim && childrenPrim.Count > 0) |
1160 | { | ||
1161 | foreach (OdePrim prm in childrenPrim) | ||
1162 | { | ||
1163 | if (m_isphysical && m_disabled) | ||
1164 | { | ||
1165 | prm.m_collisionCategories = 0; | ||
1166 | prm.m_collisionFlags = 0; | ||
1167 | } | ||
1168 | else | ||
1169 | { | ||
1170 | // preserve some | ||
1171 | if (prm.m_isSelected) | ||
1172 | { | ||
1173 | prm.m_collisionCategories = CollisionCategories.Selected; | ||
1174 | prm.m_collisionFlags = 0; | ||
1175 | } | ||
1176 | else if (prm.m_isVolumeDetect) | ||
1177 | { | ||
1178 | prm.m_collisionCategories = CollisionCategories.VolumeDtc; | ||
1179 | if (m_isphysical) | ||
1180 | prm.m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character; | ||
1181 | else | ||
1182 | prm.m_collisionFlags = 0; | ||
1183 | } | ||
1184 | else if (prm.m_isphantom) | ||
1185 | { | ||
1186 | prm.m_collisionCategories = CollisionCategories.Phantom; | ||
1187 | if (m_isphysical) | ||
1188 | prm.m_collisionFlags = CollisionCategories.Land; | ||
1189 | else | ||
1190 | prm.m_collisionFlags = 0; | ||
1191 | } | ||
1192 | else | ||
1193 | { | ||
1194 | prm.m_collisionCategories = m_collisionCategories; | ||
1195 | prm.m_collisionFlags = m_collisionFlags; | ||
1196 | } | ||
1197 | } | ||
1198 | |||
1199 | if (prm.prim_geom != IntPtr.Zero) | ||
1200 | { | ||
1201 | if (prm.m_NoColide) | ||
1202 | { | ||
1203 | d.GeomSetCategoryBits(prm.prim_geom, 0); | ||
1204 | if (m_isphysical) | ||
1205 | d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); | ||
1206 | else | ||
1207 | d.GeomSetCollideBits(prm.prim_geom, 0); | ||
1208 | } | ||
1209 | else | ||
1210 | { | ||
1211 | d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories); | ||
1212 | d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags); | ||
1213 | } | ||
1214 | } | ||
1215 | } | ||
1216 | } | ||
1217 | |||
1218 | if (m_NoColide) | ||
1219 | { | 1219 | { |
1220 | //Console.WriteLine("ChildSetGeom calls ParentPrim"); | 1220 | d.GeomSetCategoryBits(prim_geom, 0); |
1221 | AddChildPrim(prm); | 1221 | d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land); |
1222 | if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) | ||
1223 | { | ||
1224 | d.GeomSetCategoryBits(collide_geom, 0); | ||
1225 | d.GeomSetCollideBits(collide_geom, (uint)CollisionCategories.Land); | ||
1226 | } | ||
1227 | } | ||
1228 | else | ||
1229 | { | ||
1230 | d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); | ||
1231 | d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); | ||
1232 | if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) | ||
1233 | { | ||
1234 | d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories); | ||
1235 | d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags); | ||
1236 | } | ||
1222 | } | 1237 | } |
1223 | } | 1238 | } |
1224 | } | 1239 | } |
1225 | 1240 | ||
1226 | private void ChildDelink(OdePrim odePrim) | 1241 | private void createAMotor(Vector3 axis) |
1227 | { | 1242 | { |
1228 | // m_log.DebugFormat( | 1243 | if (Body == IntPtr.Zero) |
1229 | // "[ODE PRIM]: Delinking prim {0} {1} from {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID); | 1244 | return; |
1230 | 1245 | ||
1231 | // Okay, we have a delinked child.. need to rebuild the body. | 1246 | if (Amotor != IntPtr.Zero) |
1232 | lock (childrenPrim) | ||
1233 | { | 1247 | { |
1234 | foreach (OdePrim prm in childrenPrim) | 1248 | d.JointDestroy(Amotor); |
1235 | { | 1249 | Amotor = IntPtr.Zero; |
1236 | prm.childPrim = true; | ||
1237 | prm.disableBody(); | ||
1238 | //prm.m_taintparent = null; | ||
1239 | //prm._parent = null; | ||
1240 | //prm.m_taintPhysics = false; | ||
1241 | //prm.m_disabled = true; | ||
1242 | //prm.childPrim = false; | ||
1243 | } | ||
1244 | } | 1250 | } |
1245 | 1251 | ||
1246 | disableBody(); | 1252 | int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z); |
1247 | 1253 | ||
1248 | lock (childrenPrim) | 1254 | if (axisnum <= 0) |
1255 | return; | ||
1256 | |||
1257 | // stop it | ||
1258 | d.BodySetTorque(Body, 0, 0, 0); | ||
1259 | d.BodySetAngularVel(Body, 0, 0, 0); | ||
1260 | |||
1261 | Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); | ||
1262 | d.JointAttach(Amotor, Body, IntPtr.Zero); | ||
1263 | |||
1264 | d.JointSetAMotorMode(Amotor, 0); | ||
1265 | |||
1266 | d.JointSetAMotorNumAxes(Amotor, axisnum); | ||
1267 | |||
1268 | // get current orientation to lock | ||
1269 | |||
1270 | d.Quaternion dcur = d.BodyGetQuaternion(Body); | ||
1271 | Quaternion curr; // crap convertion between identical things | ||
1272 | curr.X = dcur.X; | ||
1273 | curr.Y = dcur.Y; | ||
1274 | curr.Z = dcur.Z; | ||
1275 | curr.W = dcur.W; | ||
1276 | Vector3 ax; | ||
1277 | |||
1278 | int i = 0; | ||
1279 | int j = 0; | ||
1280 | if (axis.X == 0) | ||
1249 | { | 1281 | { |
1250 | //Console.WriteLine("childrenPrim.Remove " + odePrim); | 1282 | ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X |
1251 | childrenPrim.Remove(odePrim); | 1283 | // ODE should do this with axis relative to body 1 but seems to fail |
1284 | d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z); | ||
1285 | d.JointSetAMotorAngle(Amotor, 0, 0); | ||
1286 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, -0.000001f); | ||
1287 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0.000001f); | ||
1288 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); | ||
1289 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f); | ||
1290 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f); | ||
1291 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f); | ||
1292 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f); | ||
1293 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f); | ||
1294 | i++; | ||
1295 | j = 256; // move to next axis set | ||
1252 | } | 1296 | } |
1253 | 1297 | ||
1254 | // Spurious - Body == IntPtr.Zero after disableBody() | 1298 | if (axis.Y == 0) |
1255 | // if (Body != IntPtr.Zero) | 1299 | { |
1256 | // { | 1300 | ax = (new Vector3(0, 1, 0)) * curr; |
1257 | // _parent_scene.DeactivatePrim(this); | 1301 | d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); |
1258 | // } | 1302 | d.JointSetAMotorAngle(Amotor, i, 0); |
1303 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); | ||
1304 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); | ||
1305 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); | ||
1306 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); | ||
1307 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); | ||
1308 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); | ||
1309 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); | ||
1310 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); | ||
1311 | i++; | ||
1312 | j += 256; | ||
1313 | } | ||
1259 | 1314 | ||
1260 | lock (childrenPrim) | 1315 | if (axis.Z == 0) |
1261 | { | 1316 | { |
1262 | foreach (OdePrim prm in childrenPrim) | 1317 | ax = (new Vector3(0, 0, 1)) * curr; |
1263 | { | 1318 | d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); |
1264 | //Console.WriteLine("ChildDelink calls ParentPrim"); | 1319 | d.JointSetAMotorAngle(Amotor, i, 0); |
1265 | AddChildPrim(prm); | 1320 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); |
1266 | } | 1321 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); |
1322 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); | ||
1323 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); | ||
1324 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); | ||
1325 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); | ||
1326 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); | ||
1327 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); | ||
1267 | } | 1328 | } |
1268 | } | 1329 | } |
1269 | 1330 | ||
1270 | /// <summary> | 1331 | |
1271 | /// Change prim in response to a selection taint. | 1332 | private void SetGeom(IntPtr geom) |
1272 | /// </summary> | ||
1273 | private void changeSelectedStatus() | ||
1274 | { | 1333 | { |
1275 | if (m_taintselected) | 1334 | prim_geom = geom; |
1335 | //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); | ||
1336 | if (prim_geom != IntPtr.Zero) | ||
1276 | { | 1337 | { |
1277 | m_collisionCategories = CollisionCategories.Selected; | 1338 | |
1278 | m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); | 1339 | if (m_NoColide) |
1279 | |||
1280 | // We do the body disable soft twice because 'in theory' a collision could have happened | ||
1281 | // in between the disabling and the collision properties setting | ||
1282 | // which would wake the physical body up from a soft disabling and potentially cause it to fall | ||
1283 | // through the ground. | ||
1284 | |||
1285 | // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select | ||
1286 | // just one part of the assembly, the rest of the assembly is non-selected and still simulating, | ||
1287 | // so that causes the selected part to wake up and continue moving. | ||
1288 | |||
1289 | // even if you select all parts of a jointed assembly, it is not guaranteed that the entire | ||
1290 | // assembly will stop simulating during the selection, because of the lack of atomicity | ||
1291 | // of select operations (their processing could be interrupted by a thread switch, causing | ||
1292 | // simulation to continue before all of the selected object notifications trickle down to | ||
1293 | // the physics engine). | ||
1294 | |||
1295 | // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are | ||
1296 | // selected and disabled. then, due to a thread switch, the selection processing is | ||
1297 | // interrupted and the physics engine continues to simulate, so the last 50 items, whose | ||
1298 | // selection was not yet processed, continues to simulate. this wakes up ALL of the | ||
1299 | // first 50 again. then the last 50 are disabled. then the first 50, which were just woken | ||
1300 | // up, start simulating again, which in turn wakes up the last 50. | ||
1301 | |||
1302 | if (IsPhysical) | ||
1303 | { | 1340 | { |
1304 | disableBodySoft(); | 1341 | d.GeomSetCategoryBits(prim_geom, 0); |
1342 | if (m_isphysical) | ||
1343 | { | ||
1344 | d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land); | ||
1345 | } | ||
1346 | else | ||
1347 | { | ||
1348 | d.GeomSetCollideBits(prim_geom, 0); | ||
1349 | d.GeomDisable(prim_geom); | ||
1350 | } | ||
1305 | } | 1351 | } |
1306 | 1352 | else | |
1307 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | ||
1308 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
1309 | |||
1310 | if (IsPhysical) | ||
1311 | { | 1353 | { |
1312 | disableBodySoft(); | 1354 | d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); |
1355 | d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); | ||
1313 | } | 1356 | } |
1357 | |||
1358 | UpdatePrimBodyData(); | ||
1359 | _parent_scene.actor_name_map[prim_geom] = this; | ||
1360 | |||
1361 | /* | ||
1362 | // debug | ||
1363 | d.AABB aabb; | ||
1364 | d.GeomGetAABB(prim_geom, out aabb); | ||
1365 | float x = aabb.MaxX - aabb.MinX; | ||
1366 | float y = aabb.MaxY - aabb.MinY; | ||
1367 | float z = aabb.MaxZ - aabb.MinZ; | ||
1368 | if( x > 60.0f || y > 60.0f || z > 60.0f) | ||
1369 | m_log.WarnFormat("[PHYSICS]: large prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}", | ||
1370 | Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString()); | ||
1371 | else if (x < 0.001f || y < 0.001f || z < 0.001f) | ||
1372 | m_log.WarnFormat("[PHYSICS]: small prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}", | ||
1373 | Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString()); | ||
1374 | |||
1375 | // | ||
1376 | */ | ||
1377 | |||
1314 | } | 1378 | } |
1315 | else | 1379 | else |
1380 | m_log.Warn("Setting bad Geom"); | ||
1381 | } | ||
1382 | |||
1383 | private bool GetMeshGeom() | ||
1384 | { | ||
1385 | IntPtr vertices, indices; | ||
1386 | int vertexCount, indexCount; | ||
1387 | int vertexStride, triStride; | ||
1388 | |||
1389 | IMesh mesh = m_mesh; | ||
1390 | |||
1391 | if (mesh == null) | ||
1392 | return false; | ||
1393 | |||
1394 | mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); | ||
1395 | mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); | ||
1396 | |||
1397 | if (vertexCount == 0 || indexCount == 0) | ||
1316 | { | 1398 | { |
1317 | m_collisionCategories = CollisionCategories.Geom; | 1399 | m_log.WarnFormat("[PHYSICS]: Invalid mesh data on OdePrim {0}, mesh {1}", |
1400 | Name, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh"); | ||
1401 | |||
1402 | m_hasOBB = false; | ||
1403 | m_OBBOffset = Vector3.Zero; | ||
1404 | m_OBB = _size * 0.5f; | ||
1405 | |||
1406 | m_physCost = 0.1f; | ||
1407 | m_streamCost = 1.0f; | ||
1408 | |||
1409 | _parent_scene.mesher.ReleaseMesh(mesh); | ||
1410 | m_meshState = MeshState.MeshFailed; | ||
1411 | m_mesh = null; | ||
1412 | return false; | ||
1413 | } | ||
1318 | 1414 | ||
1319 | if (IsPhysical) | 1415 | IntPtr geo = IntPtr.Zero; |
1320 | m_collisionCategories |= CollisionCategories.Body; | ||
1321 | 1416 | ||
1322 | m_collisionFlags = m_default_collisionFlags; | 1417 | try |
1418 | { | ||
1419 | _triMeshData = d.GeomTriMeshDataCreate(); | ||
1323 | 1420 | ||
1324 | if (m_collidesLand) | 1421 | d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); |
1325 | m_collisionFlags |= CollisionCategories.Land; | 1422 | d.GeomTriMeshDataPreprocess(_triMeshData); |
1326 | if (m_collidesWater) | ||
1327 | m_collisionFlags |= CollisionCategories.Water; | ||
1328 | 1423 | ||
1329 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | 1424 | geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null); |
1330 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | 1425 | } |
1331 | 1426 | ||
1332 | if (IsPhysical) | 1427 | catch (Exception e) |
1428 | { | ||
1429 | m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e); | ||
1430 | if (_triMeshData != IntPtr.Zero) | ||
1333 | { | 1431 | { |
1334 | if (Body != IntPtr.Zero) | 1432 | try |
1433 | { | ||
1434 | d.GeomTriMeshDataDestroy(_triMeshData); | ||
1435 | } | ||
1436 | catch | ||
1335 | { | 1437 | { |
1336 | d.BodySetLinearVel(Body, 0f, 0f, 0f); | ||
1337 | d.BodySetForce(Body, 0, 0, 0); | ||
1338 | enableBodySoft(); | ||
1339 | } | 1438 | } |
1340 | } | 1439 | } |
1440 | _triMeshData = IntPtr.Zero; | ||
1441 | |||
1442 | m_hasOBB = false; | ||
1443 | m_OBBOffset = Vector3.Zero; | ||
1444 | m_OBB = _size * 0.5f; | ||
1445 | m_physCost = 0.1f; | ||
1446 | m_streamCost = 1.0f; | ||
1447 | |||
1448 | _parent_scene.mesher.ReleaseMesh(mesh); | ||
1449 | m_meshState = MeshState.MeshFailed; | ||
1450 | m_mesh = null; | ||
1451 | return false; | ||
1341 | } | 1452 | } |
1342 | 1453 | ||
1343 | resetCollisionAccounting(); | 1454 | m_physCost = 0.0013f * (float)indexCount; |
1344 | m_isSelected = m_taintselected; | 1455 | // todo |
1345 | }//end changeSelectedStatus | 1456 | m_streamCost = 1.0f; |
1346 | 1457 | ||
1347 | internal void ResetTaints() | 1458 | SetGeom(geo); |
1348 | { | 1459 | |
1349 | m_taintposition = _position; | 1460 | return true; |
1350 | m_taintrot = _orientation; | ||
1351 | m_taintPhysics = IsPhysical; | ||
1352 | m_taintselected = m_isSelected; | ||
1353 | m_taintsize = _size; | ||
1354 | m_taintshape = false; | ||
1355 | m_taintforce = false; | ||
1356 | m_taintdisable = false; | ||
1357 | m_taintVelocity = Vector3.Zero; | ||
1358 | } | 1461 | } |
1359 | 1462 | ||
1360 | /// <summary> | 1463 | private void CreateGeom() |
1361 | /// Create a geometry for the given mesh in the given target space. | ||
1362 | /// </summary> | ||
1363 | /// <param name="m_targetSpace"></param> | ||
1364 | /// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param> | ||
1365 | private void CreateGeom(IntPtr m_targetSpace, IMesh mesh) | ||
1366 | { | 1464 | { |
1367 | #if SPAM | 1465 | bool hasMesh = false; |
1368 | Console.WriteLine("CreateGeom:"); | 1466 | |
1369 | #endif | 1467 | m_NoColide = false; |
1370 | if (mesh != null) | 1468 | |
1469 | if ((m_meshState & MeshState.FailMask) != 0) | ||
1470 | m_NoColide = true; | ||
1471 | |||
1472 | else if(m_mesh != null) | ||
1371 | { | 1473 | { |
1372 | setMesh(_parent_scene, mesh); | 1474 | if (GetMeshGeom()) |
1475 | hasMesh = true; | ||
1476 | else | ||
1477 | m_NoColide = true; | ||
1373 | } | 1478 | } |
1374 | else | 1479 | |
1480 | |||
1481 | if (!hasMesh) | ||
1375 | { | 1482 | { |
1376 | if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1) | 1483 | IntPtr geo = IntPtr.Zero; |
1377 | { | 1484 | |
1378 | if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z) | 1485 | if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1 |
1486 | && _size.X == _size.Y && _size.Y == _size.Z) | ||
1487 | { // it's a sphere | ||
1488 | try | ||
1379 | { | 1489 | { |
1380 | if (((_size.X / 2f) > 0f)) | 1490 | geo = d.CreateSphere(m_targetSpace, _size.X * 0.5f); |
1381 | { | ||
1382 | // _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
1383 | try | ||
1384 | { | ||
1385 | //Console.WriteLine(" CreateGeom 1"); | ||
1386 | SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2)); | ||
1387 | m_expectedCollisionContacts = 3; | ||
1388 | } | ||
1389 | catch (AccessViolationException) | ||
1390 | { | ||
1391 | m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name); | ||
1392 | return; | ||
1393 | } | ||
1394 | } | ||
1395 | else | ||
1396 | { | ||
1397 | // _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
1398 | try | ||
1399 | { | ||
1400 | //Console.WriteLine(" CreateGeom 2"); | ||
1401 | SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); | ||
1402 | m_expectedCollisionContacts = 4; | ||
1403 | } | ||
1404 | catch (AccessViolationException) | ||
1405 | { | ||
1406 | m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name); | ||
1407 | return; | ||
1408 | } | ||
1409 | } | ||
1410 | } | 1491 | } |
1411 | else | 1492 | catch (Exception e) |
1412 | { | 1493 | { |
1413 | // _parent_scene.waitForSpaceUnlock(m_targetSpace); | 1494 | m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e); |
1414 | try | 1495 | return; |
1415 | { | ||
1416 | //Console.WriteLine(" CreateGeom 3"); | ||
1417 | SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); | ||
1418 | m_expectedCollisionContacts = 4; | ||
1419 | } | ||
1420 | catch (AccessViolationException) | ||
1421 | { | ||
1422 | m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name); | ||
1423 | return; | ||
1424 | } | ||
1425 | } | 1496 | } |
1426 | } | 1497 | } |
1427 | else | 1498 | else |
1428 | { | 1499 | {// do it as a box |
1429 | // _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
1430 | try | 1500 | try |
1431 | { | 1501 | { |
1432 | //Console.WriteLine(" CreateGeom 4"); | 1502 | geo = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z); |
1433 | SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); | ||
1434 | m_expectedCollisionContacts = 4; | ||
1435 | } | 1503 | } |
1436 | catch (AccessViolationException) | 1504 | catch (Exception e) |
1437 | { | 1505 | { |
1438 | m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name); | 1506 | m_log.Warn("[PHYSICS]: Create box failed: {0}", e); |
1439 | return; | 1507 | return; |
1440 | } | 1508 | } |
1441 | } | 1509 | } |
1510 | m_physCost = 0.1f; | ||
1511 | m_streamCost = 1.0f; | ||
1512 | SetGeom(geo); | ||
1442 | } | 1513 | } |
1443 | } | 1514 | } |
1444 | 1515 | ||
1445 | /// <summary> | 1516 | private void RemoveGeom() |
1446 | /// Remove the existing geom from this prim. | ||
1447 | /// </summary> | ||
1448 | /// <param name="m_targetSpace"></param> | ||
1449 | /// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param> | ||
1450 | /// <returns>true if the geom was successfully removed, false if it was already gone or the remove failed.</returns> | ||
1451 | internal bool RemoveGeom() | ||
1452 | { | 1517 | { |
1453 | if (prim_geom != IntPtr.Zero) | 1518 | if (prim_geom != IntPtr.Zero) |
1454 | { | 1519 | { |
1520 | _parent_scene.actor_name_map.Remove(prim_geom); | ||
1521 | |||
1455 | try | 1522 | try |
1456 | { | 1523 | { |
1457 | _parent_scene.geom_name_map.Remove(prim_geom); | ||
1458 | _parent_scene.actor_name_map.Remove(prim_geom); | ||
1459 | d.GeomDestroy(prim_geom); | 1524 | d.GeomDestroy(prim_geom); |
1460 | m_expectedCollisionContacts = 0; | 1525 | if (_triMeshData != IntPtr.Zero) |
1461 | prim_geom = IntPtr.Zero; | 1526 | { |
1527 | d.GeomTriMeshDataDestroy(_triMeshData); | ||
1528 | _triMeshData = IntPtr.Zero; | ||
1529 | } | ||
1462 | } | 1530 | } |
1463 | catch (System.AccessViolationException) | 1531 | catch (Exception e) |
1464 | { | 1532 | { |
1465 | prim_geom = IntPtr.Zero; | 1533 | m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e); |
1466 | m_expectedCollisionContacts = 0; | ||
1467 | m_log.ErrorFormat("[PHYSICS]: PrimGeom dead for {0}", Name); | ||
1468 | |||
1469 | return false; | ||
1470 | } | 1534 | } |
1471 | 1535 | ||
1472 | return true; | 1536 | prim_geom = IntPtr.Zero; |
1537 | collide_geom = IntPtr.Zero; | ||
1538 | m_targetSpace = IntPtr.Zero; | ||
1473 | } | 1539 | } |
1474 | else | 1540 | else |
1475 | { | 1541 | { |
1476 | m_log.WarnFormat( | 1542 | m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name); |
1477 | "[ODE PRIM]: Called RemoveGeom() on {0} {1} where geometry was already null.", Name, LocalID); | 1543 | } |
1478 | 1544 | ||
1479 | return false; | 1545 | lock (m_meshlock) |
1546 | { | ||
1547 | if (m_mesh != null) | ||
1548 | { | ||
1549 | _parent_scene.mesher.ReleaseMesh(m_mesh); | ||
1550 | m_mesh = null; | ||
1551 | } | ||
1480 | } | 1552 | } |
1553 | |||
1554 | Body = IntPtr.Zero; | ||
1555 | m_hasOBB = false; | ||
1481 | } | 1556 | } |
1482 | /// <summary> | 1557 | |
1483 | /// Add prim in response to an add taint. | 1558 | //sets non physical prim m_targetSpace to right space in spaces grid for static prims |
1484 | /// </summary> | 1559 | // should only be called for non physical prims unless they are becoming non physical |
1485 | private void changeadd() | 1560 | private void SetInStaticSpace(OdePrim prim) |
1486 | { | 1561 | { |
1487 | // m_log.DebugFormat("[ODE PRIM]: Adding prim {0}", Name); | 1562 | IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace); |
1488 | 1563 | prim.m_targetSpace = targetSpace; | |
1489 | int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position); | 1564 | collide_geom = IntPtr.Zero; |
1490 | IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position); | 1565 | } |
1491 | 1566 | ||
1492 | if (targetspace == IntPtr.Zero) | 1567 | public void enableBodySoft() |
1493 | targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]); | 1568 | { |
1569 | m_disabled = false; | ||
1570 | if (!childPrim && !m_isSelected) | ||
1571 | { | ||
1572 | if (m_isphysical && Body != IntPtr.Zero) | ||
1573 | { | ||
1574 | UpdateCollisionCatFlags(); | ||
1575 | ApplyCollisionCatFlags(); | ||
1494 | 1576 | ||
1495 | m_targetSpace = targetspace; | 1577 | d.BodyEnable(Body); |
1578 | } | ||
1579 | } | ||
1580 | resetCollisionAccounting(); | ||
1581 | } | ||
1496 | 1582 | ||
1497 | IMesh mesh = null; | 1583 | private void disableBodySoft() |
1584 | { | ||
1585 | m_disabled = true; | ||
1586 | if (!childPrim) | ||
1587 | { | ||
1588 | if (m_isphysical && Body != IntPtr.Zero) | ||
1589 | { | ||
1590 | if (m_isSelected) | ||
1591 | m_collisionFlags = CollisionCategories.Selected; | ||
1592 | else | ||
1593 | m_collisionCategories = 0; | ||
1594 | m_collisionFlags = 0; | ||
1595 | ApplyCollisionCatFlags(); | ||
1596 | d.BodyDisable(Body); | ||
1597 | } | ||
1598 | } | ||
1599 | } | ||
1600 | |||
1601 | private void MakeBody() | ||
1602 | { | ||
1603 | if (!m_isphysical) // only physical get bodies | ||
1604 | return; | ||
1605 | |||
1606 | if (childPrim) // child prims don't get bodies; | ||
1607 | return; | ||
1608 | |||
1609 | if (m_building) | ||
1610 | return; | ||
1611 | |||
1612 | if (prim_geom == IntPtr.Zero) | ||
1613 | { | ||
1614 | m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet"); | ||
1615 | return; | ||
1616 | } | ||
1498 | 1617 | ||
1499 | if (_parent_scene.needsMeshing(_pbs)) | 1618 | if (Body != IntPtr.Zero) |
1500 | { | 1619 | { |
1501 | // Don't need to re-enable body.. it's done in SetMesh | 1620 | DestroyBody(); |
1502 | mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical); | 1621 | m_log.Warn("[PHYSICS]: MakeBody called having a body"); |
1503 | // createmesh returns null when it's a shape that isn't a cube. | ||
1504 | // m_log.Debug(m_localID); | ||
1505 | if (mesh == null) | ||
1506 | CheckMeshAsset(); | ||
1507 | } | 1622 | } |
1508 | 1623 | ||
1509 | #if SPAM | 1624 | if (d.GeomGetBody(prim_geom) != IntPtr.Zero) |
1510 | Console.WriteLine("changeadd 1"); | 1625 | { |
1511 | #endif | 1626 | d.GeomSetBody(prim_geom, IntPtr.Zero); |
1512 | CreateGeom(m_targetSpace, mesh); | 1627 | m_log.Warn("[PHYSICS]: MakeBody root geom already had a body"); |
1628 | } | ||
1513 | 1629 | ||
1514 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); | 1630 | d.Matrix3 mymat = new d.Matrix3(); |
1515 | d.Quaternion myrot = new d.Quaternion(); | 1631 | d.Quaternion myrot = new d.Quaternion(); |
1632 | d.Mass objdmass = new d.Mass { }; | ||
1633 | |||
1634 | Body = d.BodyCreate(_parent_scene.world); | ||
1635 | |||
1636 | objdmass = primdMass; | ||
1637 | |||
1638 | // rotate inertia | ||
1516 | myrot.X = _orientation.X; | 1639 | myrot.X = _orientation.X; |
1517 | myrot.Y = _orientation.Y; | 1640 | myrot.Y = _orientation.Y; |
1518 | myrot.Z = _orientation.Z; | 1641 | myrot.Z = _orientation.Z; |
1519 | myrot.W = _orientation.W; | 1642 | myrot.W = _orientation.W; |
1520 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
1521 | 1643 | ||
1522 | if (IsPhysical && Body == IntPtr.Zero) | 1644 | d.RfromQ(out mymat, ref myrot); |
1523 | enableBody(); | 1645 | d.MassRotate(ref objdmass, ref mymat); |
1524 | 1646 | ||
1525 | changeSelectedStatus(); | 1647 | // set the body rotation |
1526 | 1648 | d.BodySetRotation(Body, ref mymat); | |
1527 | m_taintadd = false; | ||
1528 | } | ||
1529 | 1649 | ||
1530 | /// <summary> | 1650 | // recompute full object inertia if needed |
1531 | /// Move prim in response to a move taint. | 1651 | if (childrenPrim.Count > 0) |
1532 | /// </summary> | ||
1533 | private void changemove() | ||
1534 | { | ||
1535 | if (IsPhysical) | ||
1536 | { | 1652 | { |
1537 | if (!m_disabled && !m_taintremove && !childPrim) | 1653 | d.Matrix3 mat = new d.Matrix3(); |
1538 | { | 1654 | d.Quaternion quat = new d.Quaternion(); |
1539 | if (Body == IntPtr.Zero) | 1655 | d.Mass tmpdmass = new d.Mass { }; |
1540 | enableBody(); | 1656 | Vector3 rcm; |
1541 | 1657 | ||
1542 | //Prim auto disable after 20 frames, | 1658 | rcm.X = _position.X; |
1543 | //if you move it, re-enable the prim manually. | 1659 | rcm.Y = _position.Y; |
1544 | if (_parent != null) | 1660 | rcm.Z = _position.Z; |
1545 | { | ||
1546 | if (m_linkJoint != IntPtr.Zero) | ||
1547 | { | ||
1548 | d.JointDestroy(m_linkJoint); | ||
1549 | m_linkJoint = IntPtr.Zero; | ||
1550 | } | ||
1551 | } | ||
1552 | 1661 | ||
1553 | if (Body != IntPtr.Zero) | 1662 | lock (childrenPrim) |
1663 | { | ||
1664 | foreach (OdePrim prm in childrenPrim) | ||
1554 | { | 1665 | { |
1555 | d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); | 1666 | if (prm.prim_geom == IntPtr.Zero) |
1556 | |||
1557 | if (_parent != null) | ||
1558 | { | 1667 | { |
1559 | OdePrim odParent = (OdePrim)_parent; | 1668 | m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet"); |
1560 | if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body) | 1669 | continue; |
1561 | { | ||
1562 | // KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used?? | ||
1563 | Console.WriteLine(" JointCreateFixed"); | ||
1564 | m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); | ||
1565 | d.JointAttach(m_linkJoint, Body, odParent.Body); | ||
1566 | d.JointSetFixed(m_linkJoint); | ||
1567 | } | ||
1568 | } | 1670 | } |
1569 | d.BodyEnable(Body); | 1671 | |
1570 | if (m_vehicle.Type != Vehicle.TYPE_NONE) | 1672 | tmpdmass = prm.primdMass; |
1673 | |||
1674 | // apply prim current rotation to inertia | ||
1675 | quat.X = prm._orientation.X; | ||
1676 | quat.Y = prm._orientation.Y; | ||
1677 | quat.Z = prm._orientation.Z; | ||
1678 | quat.W = prm._orientation.W; | ||
1679 | d.RfromQ(out mat, ref quat); | ||
1680 | d.MassRotate(ref tmpdmass, ref mat); | ||
1681 | |||
1682 | Vector3 ppos = prm._position; | ||
1683 | ppos.X -= rcm.X; | ||
1684 | ppos.Y -= rcm.Y; | ||
1685 | ppos.Z -= rcm.Z; | ||
1686 | // refer inertia to root prim center of mass position | ||
1687 | d.MassTranslate(ref tmpdmass, | ||
1688 | ppos.X, | ||
1689 | ppos.Y, | ||
1690 | ppos.Z); | ||
1691 | |||
1692 | d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia | ||
1693 | // fix prim colision cats | ||
1694 | |||
1695 | if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero) | ||
1571 | { | 1696 | { |
1572 | m_vehicle.Enable(Body, _parent_scene); | 1697 | d.GeomSetBody(prm.prim_geom, IntPtr.Zero); |
1698 | m_log.Warn("[PHYSICS]: MakeBody child geom already had a body"); | ||
1573 | } | 1699 | } |
1574 | } | 1700 | |
1575 | else | 1701 | d.GeomClearOffset(prm.prim_geom); |
1576 | { | 1702 | d.GeomSetBody(prm.prim_geom, Body); |
1577 | m_log.WarnFormat("[PHYSICS]: Body for {0} still null after enableBody(). This is a crash scenario.", Name); | 1703 | prm.Body = Body; |
1704 | d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation | ||
1578 | } | 1705 | } |
1579 | } | 1706 | } |
1580 | //else | ||
1581 | // { | ||
1582 | //m_log.Debug("[BUG]: race!"); | ||
1583 | //} | ||
1584 | } | 1707 | } |
1585 | 1708 | ||
1586 | // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); | 1709 | d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset |
1587 | // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); | 1710 | // associate root geom with body |
1588 | // _parent_scene.waitForSpaceUnlock(m_targetSpace); | 1711 | d.GeomSetBody(prim_geom, Body); |
1712 | |||
1713 | d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z); | ||
1714 | d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z); | ||
1589 | 1715 | ||
1590 | IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace); | 1716 | d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body |
1591 | m_targetSpace = tempspace; | 1717 | myrot.X = -myrot.X; |
1718 | myrot.Y = -myrot.Y; | ||
1719 | myrot.Z = -myrot.Z; | ||
1592 | 1720 | ||
1593 | // _parent_scene.waitForSpaceUnlock(m_targetSpace); | 1721 | d.RfromQ(out mymat, ref myrot); |
1722 | d.MassRotate(ref objdmass, ref mymat); | ||
1594 | 1723 | ||
1595 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); | 1724 | d.BodySetMass(Body, ref objdmass); |
1725 | _mass = objdmass.mass; | ||
1726 | |||
1727 | // disconnect from world gravity so we can apply buoyancy | ||
1728 | d.BodySetGravityMode(Body, false); | ||
1729 | |||
1730 | d.BodySetAutoDisableFlag(Body, true); | ||
1731 | d.BodySetAutoDisableSteps(Body, body_autodisable_frames); | ||
1732 | d.BodySetDamping(Body, .005f, .005f); | ||
1733 | |||
1734 | if (m_targetSpace != IntPtr.Zero) | ||
1735 | { | ||
1736 | _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
1737 | if (d.SpaceQuery(m_targetSpace, prim_geom)) | ||
1738 | d.SpaceRemove(m_targetSpace, prim_geom); | ||
1739 | } | ||
1740 | |||
1741 | if (childrenPrim.Count == 0) | ||
1742 | { | ||
1743 | collide_geom = prim_geom; | ||
1744 | m_targetSpace = _parent_scene.ActiveSpace; | ||
1745 | } | ||
1746 | else | ||
1747 | { | ||
1748 | m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace); | ||
1749 | d.HashSpaceSetLevels(m_targetSpace, -2, 8); | ||
1750 | d.SpaceSetSublevel(m_targetSpace, 3); | ||
1751 | d.SpaceSetCleanup(m_targetSpace, false); | ||
1752 | |||
1753 | d.GeomSetCategoryBits(m_targetSpace, (uint)(CollisionCategories.Space | | ||
1754 | CollisionCategories.Geom | | ||
1755 | CollisionCategories.Phantom | | ||
1756 | CollisionCategories.VolumeDtc | ||
1757 | )); | ||
1758 | d.GeomSetCollideBits(m_targetSpace, 0); | ||
1759 | collide_geom = m_targetSpace; | ||
1760 | } | ||
1596 | 1761 | ||
1597 | // _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
1598 | d.SpaceAdd(m_targetSpace, prim_geom); | 1762 | d.SpaceAdd(m_targetSpace, prim_geom); |
1599 | 1763 | ||
1600 | changeSelectedStatus(); | 1764 | if (m_delaySelect) |
1765 | { | ||
1766 | m_isSelected = true; | ||
1767 | m_delaySelect = false; | ||
1768 | } | ||
1601 | 1769 | ||
1602 | resetCollisionAccounting(); | 1770 | m_collisionscore = 0; |
1603 | m_taintposition = _position; | ||
1604 | } | ||
1605 | 1771 | ||
1606 | internal void Move(float timestep) | 1772 | UpdateCollisionCatFlags(); |
1607 | { | 1773 | ApplyCollisionCatFlags(); |
1608 | float fx = 0; | ||
1609 | float fy = 0; | ||
1610 | float fz = 0; | ||
1611 | 1774 | ||
1612 | if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. | 1775 | _parent_scene.addActivePrim(this); |
1776 | |||
1777 | lock (childrenPrim) | ||
1613 | { | 1778 | { |
1614 | if (m_vehicle.Type != Vehicle.TYPE_NONE) | 1779 | foreach (OdePrim prm in childrenPrim) |
1615 | { | ||
1616 | // 'VEHICLES' are dealt with in ODEDynamics.cs | ||
1617 | m_vehicle.Step(timestep, _parent_scene); | ||
1618 | } | ||
1619 | else | ||
1620 | { | 1780 | { |
1621 | //Console.WriteLine("Move " + Name); | 1781 | if (prm.prim_geom == IntPtr.Zero) |
1622 | if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 | 1782 | continue; |
1623 | // NON-'VEHICLES' are dealt with here | ||
1624 | // if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f)) | ||
1625 | // { | ||
1626 | // d.Vector3 avel2 = d.BodyGetAngularVel(Body); | ||
1627 | // /* | ||
1628 | // if (m_angularlock.X == 1) | ||
1629 | // avel2.X = 0; | ||
1630 | // if (m_angularlock.Y == 1) | ||
1631 | // avel2.Y = 0; | ||
1632 | // if (m_angularlock.Z == 1) | ||
1633 | // avel2.Z = 0; | ||
1634 | // d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z); | ||
1635 | // */ | ||
1636 | // } | ||
1637 | //float PID_P = 900.0f; | ||
1638 | |||
1639 | float m_mass = CalculateMass(); | ||
1640 | |||
1641 | // fz = 0f; | ||
1642 | //m_log.Info(m_collisionFlags.ToString()); | ||
1643 | 1783 | ||
1644 | 1784 | Vector3 ppos = prm._position; | |
1645 | //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. | 1785 | d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position |
1646 | // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ?? | ||
1647 | // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up | ||
1648 | // gravityz multiplier = 1 - m_buoyancy | ||
1649 | fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; | ||
1650 | 1786 | ||
1651 | if (m_usePID) | 1787 | if (prm.m_targetSpace != m_targetSpace) |
1652 | { | 1788 | { |
1653 | //Console.WriteLine("PID " + Name); | 1789 | if (prm.m_targetSpace != IntPtr.Zero) |
1654 | // KF - this is for object move? eg. llSetPos() ? | ||
1655 | //if (!d.BodyIsEnabled(Body)) | ||
1656 | //d.BodySetForce(Body, 0f, 0f, 0f); | ||
1657 | // If we're using the PID controller, then we have no gravity | ||
1658 | //fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply... | ||
1659 | fz = 0f; | ||
1660 | |||
1661 | // no lock; for now it's only called from within Simulate() | ||
1662 | |||
1663 | // If the PID Controller isn't active then we set our force | ||
1664 | // calculating base velocity to the current position | ||
1665 | |||
1666 | if ((m_PIDTau < 1) && (m_PIDTau != 0)) | ||
1667 | { | ||
1668 | //PID_G = PID_G / m_PIDTau; | ||
1669 | m_PIDTau = 1; | ||
1670 | } | ||
1671 | |||
1672 | if ((PID_G - m_PIDTau) <= 0) | ||
1673 | { | 1790 | { |
1674 | PID_G = m_PIDTau + 1; | 1791 | _parent_scene.waitForSpaceUnlock(prm.m_targetSpace); |
1792 | if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom)) | ||
1793 | d.SpaceRemove(prm.m_targetSpace, prm.prim_geom); | ||
1675 | } | 1794 | } |
1676 | //PidStatus = true; | 1795 | prm.m_targetSpace = m_targetSpace; |
1796 | d.SpaceAdd(m_targetSpace, prm.prim_geom); | ||
1797 | } | ||
1677 | 1798 | ||
1678 | // PhysicsVector vec = new PhysicsVector(); | 1799 | prm.m_collisionscore = 0; |
1679 | d.Vector3 vel = d.BodyGetLinearVel(Body); | ||
1680 | 1800 | ||
1681 | d.Vector3 pos = d.BodyGetPosition(Body); | 1801 | if(!m_disabled) |
1682 | _target_velocity = | 1802 | prm.m_disabled = false; |
1683 | new Vector3( | ||
1684 | (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), | ||
1685 | (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), | ||
1686 | (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) | ||
1687 | ); | ||
1688 | 1803 | ||
1689 | // if velocity is zero, use position control; otherwise, velocity control | 1804 | _parent_scene.addActivePrim(prm); |
1805 | } | ||
1806 | } | ||
1690 | 1807 | ||
1691 | if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f)) | 1808 | // The body doesn't already have a finite rotation mode set here |
1692 | { | 1809 | if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null) |
1693 | // keep track of where we stopped. No more slippin' & slidin' | 1810 | { |
1694 | 1811 | createAMotor(m_angularlock); | |
1695 | // We only want to deactivate the PID Controller if we think we want to have our surrogate | 1812 | } |
1696 | // react to the physics scene by moving it's position. | ||
1697 | // Avatar to Avatar collisions | ||
1698 | // Prim to avatar collisions | ||
1699 | |||
1700 | //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); | ||
1701 | //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); | ||
1702 | //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; | ||
1703 | d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); | ||
1704 | d.BodySetLinearVel(Body, 0, 0, 0); | ||
1705 | d.BodyAddForce(Body, 0, 0, fz); | ||
1706 | return; | ||
1707 | } | ||
1708 | else | ||
1709 | { | ||
1710 | _zeroFlag = false; | ||
1711 | 1813 | ||
1712 | // We're flying and colliding with something | ||
1713 | fx = ((_target_velocity.X) - vel.X) * (PID_D); | ||
1714 | fy = ((_target_velocity.Y) - vel.Y) * (PID_D); | ||
1715 | |||
1716 | // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; | ||
1717 | 1814 | ||
1718 | fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); | 1815 | if (m_isSelected || m_disabled) |
1719 | } | 1816 | { |
1720 | } // end if (m_usePID) | 1817 | d.BodyDisable(Body); |
1818 | } | ||
1819 | else | ||
1820 | { | ||
1821 | d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z); | ||
1822 | d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); | ||
1823 | } | ||
1824 | _parent_scene.addActiveGroups(this); | ||
1825 | } | ||
1721 | 1826 | ||
1722 | // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller | 1827 | private void DestroyBody() |
1723 | if (m_useHoverPID && !m_usePID) | 1828 | { |
1724 | { | 1829 | if (Body != IntPtr.Zero) |
1725 | //Console.WriteLine("Hover " + Name); | 1830 | { |
1726 | 1831 | _parent_scene.remActivePrim(this); | |
1727 | // If we're using the PID controller, then we have no gravity | ||
1728 | fz = (-1 * _parent_scene.gravityz) * m_mass; | ||
1729 | 1832 | ||
1730 | // no lock; for now it's only called from within Simulate() | 1833 | collide_geom = IntPtr.Zero; |
1731 | 1834 | ||
1732 | // If the PID Controller isn't active then we set our force | 1835 | if (m_disabled) |
1733 | // calculating base velocity to the current position | 1836 | m_collisionCategories = 0; |
1837 | else if (m_isSelected) | ||
1838 | m_collisionCategories = CollisionCategories.Selected; | ||
1839 | else if (m_isVolumeDetect) | ||
1840 | m_collisionCategories = CollisionCategories.VolumeDtc; | ||
1841 | else if (m_isphantom) | ||
1842 | m_collisionCategories = CollisionCategories.Phantom; | ||
1843 | else | ||
1844 | m_collisionCategories = CollisionCategories.Geom; | ||
1734 | 1845 | ||
1735 | if ((m_PIDTau < 1)) | 1846 | m_collisionFlags = 0; |
1736 | { | 1847 | |
1737 | PID_G = PID_G / m_PIDTau; | 1848 | if (prim_geom != IntPtr.Zero) |
1738 | } | 1849 | { |
1850 | if (m_NoColide) | ||
1851 | { | ||
1852 | d.GeomSetCategoryBits(prim_geom, 0); | ||
1853 | d.GeomSetCollideBits(prim_geom, 0); | ||
1854 | } | ||
1855 | else | ||
1856 | { | ||
1857 | d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); | ||
1858 | d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); | ||
1859 | } | ||
1860 | UpdateDataFromGeom(); | ||
1861 | d.GeomSetBody(prim_geom, IntPtr.Zero); | ||
1862 | SetInStaticSpace(this); | ||
1863 | } | ||
1739 | 1864 | ||
1740 | if ((PID_G - m_PIDTau) <= 0) | 1865 | if (!childPrim) |
1866 | { | ||
1867 | lock (childrenPrim) | ||
1868 | { | ||
1869 | foreach (OdePrim prm in childrenPrim) | ||
1741 | { | 1870 | { |
1742 | PID_G = m_PIDTau + 1; | 1871 | _parent_scene.remActivePrim(prm); |
1743 | } | ||
1744 | 1872 | ||
1745 | // Where are we, and where are we headed? | 1873 | if (prm.m_isSelected) |
1746 | d.Vector3 pos = d.BodyGetPosition(Body); | 1874 | prm.m_collisionCategories = CollisionCategories.Selected; |
1747 | d.Vector3 vel = d.BodyGetLinearVel(Body); | 1875 | else if (prm.m_isVolumeDetect) |
1876 | prm.m_collisionCategories = CollisionCategories.VolumeDtc; | ||
1877 | else if (prm.m_isphantom) | ||
1878 | prm.m_collisionCategories = CollisionCategories.Phantom; | ||
1879 | else | ||
1880 | prm.m_collisionCategories = CollisionCategories.Geom; | ||
1748 | 1881 | ||
1749 | // Non-Vehicles have a limited set of Hover options. | 1882 | prm.m_collisionFlags = 0; |
1750 | // determine what our target height really is based on HoverType | 1883 | |
1751 | switch (m_PIDHoverType) | 1884 | if (prm.prim_geom != IntPtr.Zero) |
1752 | { | 1885 | { |
1753 | case PIDHoverType.Ground: | 1886 | if (prm.m_NoColide) |
1754 | m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); | ||
1755 | m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; | ||
1756 | break; | ||
1757 | case PIDHoverType.GroundAndWater: | ||
1758 | m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); | ||
1759 | m_waterHeight = _parent_scene.GetWaterLevel(); | ||
1760 | if (m_groundHeight > m_waterHeight) | ||
1761 | { | 1887 | { |
1762 | m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; | 1888 | d.GeomSetCategoryBits(prm.prim_geom, 0); |
1889 | d.GeomSetCollideBits(prm.prim_geom, 0); | ||
1763 | } | 1890 | } |
1764 | else | 1891 | else |
1765 | { | 1892 | { |
1766 | m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; | 1893 | d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories); |
1894 | d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags); | ||
1767 | } | 1895 | } |
1768 | break; | 1896 | prm.UpdateDataFromGeom(); |
1897 | SetInStaticSpace(prm); | ||
1898 | } | ||
1899 | prm.Body = IntPtr.Zero; | ||
1900 | prm._mass = prm.primMass; | ||
1901 | prm.m_collisionscore = 0; | ||
1902 | } | ||
1903 | } | ||
1904 | if (Amotor != IntPtr.Zero) | ||
1905 | { | ||
1906 | d.JointDestroy(Amotor); | ||
1907 | Amotor = IntPtr.Zero; | ||
1908 | } | ||
1909 | _parent_scene.remActiveGroup(this); | ||
1910 | d.BodyDestroy(Body); | ||
1911 | } | ||
1912 | Body = IntPtr.Zero; | ||
1913 | } | ||
1914 | _mass = primMass; | ||
1915 | m_collisionscore = 0; | ||
1916 | } | ||
1769 | 1917 | ||
1770 | } // end switch (m_PIDHoverType) | 1918 | private void FixInertia(Vector3 NewPos,Quaternion newrot) |
1919 | { | ||
1920 | d.Matrix3 mat = new d.Matrix3(); | ||
1921 | d.Quaternion quat = new d.Quaternion(); | ||
1771 | 1922 | ||
1923 | d.Mass tmpdmass = new d.Mass { }; | ||
1924 | d.Mass objdmass = new d.Mass { }; | ||
1772 | 1925 | ||
1773 | _target_velocity = | 1926 | d.BodyGetMass(Body, out tmpdmass); |
1774 | new Vector3(0.0f, 0.0f, | 1927 | objdmass = tmpdmass; |
1775 | (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) | ||
1776 | ); | ||
1777 | 1928 | ||
1778 | // if velocity is zero, use position control; otherwise, velocity control | 1929 | d.Vector3 dobjpos; |
1930 | d.Vector3 thispos; | ||
1779 | 1931 | ||
1780 | if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) | 1932 | // get current object position and rotation |
1781 | { | 1933 | dobjpos = d.BodyGetPosition(Body); |
1782 | // keep track of where we stopped. No more slippin' & slidin' | ||
1783 | |||
1784 | // We only want to deactivate the PID Controller if we think we want to have our surrogate | ||
1785 | // react to the physics scene by moving it's position. | ||
1786 | // Avatar to Avatar collisions | ||
1787 | // Prim to avatar collisions | ||
1788 | |||
1789 | d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); | ||
1790 | d.BodySetLinearVel(Body, vel.X, vel.Y, 0); | ||
1791 | d.BodyAddForce(Body, 0, 0, fz); | ||
1792 | return; | ||
1793 | } | ||
1794 | else | ||
1795 | { | ||
1796 | _zeroFlag = false; | ||
1797 | 1934 | ||
1798 | // We're flying and colliding with something | 1935 | // get prim own inertia in its local frame |
1799 | fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); | 1936 | tmpdmass = primdMass; |
1800 | } | ||
1801 | } | ||
1802 | 1937 | ||
1803 | fx *= m_mass; | 1938 | // transform to object frame |
1804 | fy *= m_mass; | 1939 | mat = d.GeomGetOffsetRotation(prim_geom); |
1805 | //fz *= m_mass; | 1940 | d.MassRotate(ref tmpdmass, ref mat); |
1806 | 1941 | ||
1807 | fx += m_force.X; | 1942 | thispos = d.GeomGetOffsetPosition(prim_geom); |
1808 | fy += m_force.Y; | 1943 | d.MassTranslate(ref tmpdmass, |
1809 | fz += m_force.Z; | 1944 | thispos.X, |
1945 | thispos.Y, | ||
1946 | thispos.Z); | ||
1810 | 1947 | ||
1811 | //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); | 1948 | // subtract current prim inertia from object |
1812 | if (fx != 0 || fy != 0 || fz != 0) | 1949 | DMassSubPartFromObj(ref tmpdmass, ref objdmass); |
1813 | { | ||
1814 | //m_taintdisable = true; | ||
1815 | //base.RaiseOutOfBounds(Position); | ||
1816 | //d.BodySetLinearVel(Body, fx, fy, 0f); | ||
1817 | if (!d.BodyIsEnabled(Body)) | ||
1818 | { | ||
1819 | // A physical body at rest on a surface will auto-disable after a while, | ||
1820 | // this appears to re-enable it incase the surface it is upon vanishes, | ||
1821 | // and the body should fall again. | ||
1822 | d.BodySetLinearVel(Body, 0f, 0f, 0f); | ||
1823 | d.BodySetForce(Body, 0, 0, 0); | ||
1824 | enableBodySoft(); | ||
1825 | } | ||
1826 | 1950 | ||
1827 | // 35x10 = 350n times the mass per second applied maximum. | 1951 | // back prim own inertia |
1828 | float nmax = 35f * m_mass; | 1952 | tmpdmass = primdMass; |
1829 | float nmin = -35f * m_mass; | 1953 | |
1830 | 1954 | // update to new position and orientation | |
1831 | if (fx > nmax) | 1955 | _position = NewPos; |
1832 | fx = nmax; | 1956 | d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z); |
1833 | if (fx < nmin) | 1957 | _orientation = newrot; |
1834 | fx = nmin; | 1958 | quat.X = newrot.X; |
1835 | if (fy > nmax) | 1959 | quat.Y = newrot.Y; |
1836 | fy = nmax; | 1960 | quat.Z = newrot.Z; |
1837 | if (fy < nmin) | 1961 | quat.W = newrot.W; |
1838 | fy = nmin; | 1962 | d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat); |
1839 | d.BodyAddForce(Body, fx, fy, fz); | 1963 | |
1840 | //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); | 1964 | mat = d.GeomGetOffsetRotation(prim_geom); |
1841 | } | 1965 | d.MassRotate(ref tmpdmass, ref mat); |
1842 | } | 1966 | |
1843 | } | 1967 | thispos = d.GeomGetOffsetPosition(prim_geom); |
1844 | else | 1968 | d.MassTranslate(ref tmpdmass, |
1845 | { // is not physical, or is not a body or is selected | 1969 | thispos.X, |
1846 | // _zeroPosition = d.BodyGetPosition(Body); | 1970 | thispos.Y, |
1847 | return; | 1971 | thispos.Z); |
1848 | //Console.WriteLine("Nothing " + Name); | 1972 | |
1849 | 1973 | d.MassAdd(ref objdmass, ref tmpdmass); | |
1974 | |||
1975 | // fix all positions | ||
1976 | IntPtr g = d.BodyGetFirstGeom(Body); | ||
1977 | while (g != IntPtr.Zero) | ||
1978 | { | ||
1979 | thispos = d.GeomGetOffsetPosition(g); | ||
1980 | thispos.X -= objdmass.c.X; | ||
1981 | thispos.Y -= objdmass.c.Y; | ||
1982 | thispos.Z -= objdmass.c.Z; | ||
1983 | d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); | ||
1984 | g = d.dBodyGetNextGeom(g); | ||
1850 | } | 1985 | } |
1986 | d.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos); | ||
1987 | |||
1988 | d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); | ||
1989 | d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body | ||
1990 | d.BodySetMass(Body, ref objdmass); | ||
1991 | _mass = objdmass.mass; | ||
1851 | } | 1992 | } |
1852 | 1993 | ||
1853 | private void rotate() | 1994 | |
1995 | |||
1996 | private void FixInertia(Vector3 NewPos) | ||
1854 | { | 1997 | { |
1855 | d.Quaternion myrot = new d.Quaternion(); | 1998 | d.Matrix3 primmat = new d.Matrix3(); |
1856 | myrot.X = _orientation.X; | 1999 | d.Mass tmpdmass = new d.Mass { }; |
1857 | myrot.Y = _orientation.Y; | 2000 | d.Mass objdmass = new d.Mass { }; |
1858 | myrot.Z = _orientation.Z; | 2001 | d.Mass primmass = new d.Mass { }; |
1859 | myrot.W = _orientation.W; | 2002 | |
1860 | if (Body != IntPtr.Zero) | 2003 | d.Vector3 dobjpos; |
1861 | { | 2004 | d.Vector3 thispos; |
1862 | // KF: If this is a root prim do BodySet | 2005 | |
1863 | d.BodySetQuaternion(Body, ref myrot); | 2006 | d.BodyGetMass(Body, out objdmass); |
1864 | if (IsPhysical) | 2007 | |
1865 | { | 2008 | // get prim own inertia in its local frame |
1866 | if (!m_angularlock.ApproxEquals(Vector3.One, 0f)) | 2009 | primmass = primdMass; |
1867 | createAMotor(m_angularlock); | 2010 | // transform to object frame |
1868 | } | 2011 | primmat = d.GeomGetOffsetRotation(prim_geom); |
1869 | } | 2012 | d.MassRotate(ref primmass, ref primmat); |
1870 | else | 2013 | |
2014 | tmpdmass = primmass; | ||
2015 | |||
2016 | thispos = d.GeomGetOffsetPosition(prim_geom); | ||
2017 | d.MassTranslate(ref tmpdmass, | ||
2018 | thispos.X, | ||
2019 | thispos.Y, | ||
2020 | thispos.Z); | ||
2021 | |||
2022 | // subtract current prim inertia from object | ||
2023 | DMassSubPartFromObj(ref tmpdmass, ref objdmass); | ||
2024 | |||
2025 | // update to new position | ||
2026 | _position = NewPos; | ||
2027 | d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z); | ||
2028 | |||
2029 | thispos = d.GeomGetOffsetPosition(prim_geom); | ||
2030 | d.MassTranslate(ref primmass, | ||
2031 | thispos.X, | ||
2032 | thispos.Y, | ||
2033 | thispos.Z); | ||
2034 | |||
2035 | d.MassAdd(ref objdmass, ref primmass); | ||
2036 | |||
2037 | // fix all positions | ||
2038 | IntPtr g = d.BodyGetFirstGeom(Body); | ||
2039 | while (g != IntPtr.Zero) | ||
1871 | { | 2040 | { |
1872 | // daughter prim, do Geom set | 2041 | thispos = d.GeomGetOffsetPosition(g); |
1873 | d.GeomSetQuaternion(prim_geom, ref myrot); | 2042 | thispos.X -= objdmass.c.X; |
2043 | thispos.Y -= objdmass.c.Y; | ||
2044 | thispos.Z -= objdmass.c.Z; | ||
2045 | d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); | ||
2046 | g = d.dBodyGetNextGeom(g); | ||
1874 | } | 2047 | } |
1875 | 2048 | ||
1876 | resetCollisionAccounting(); | 2049 | d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos); |
1877 | m_taintrot = _orientation; | 2050 | |
2051 | // get current object position and rotation | ||
2052 | dobjpos = d.BodyGetPosition(Body); | ||
2053 | |||
2054 | d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); | ||
2055 | d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body | ||
2056 | d.BodySetMass(Body, ref objdmass); | ||
2057 | _mass = objdmass.mass; | ||
1878 | } | 2058 | } |
1879 | 2059 | ||
1880 | private void resetCollisionAccounting() | 2060 | private void FixInertia(Quaternion newrot) |
1881 | { | 2061 | { |
1882 | m_collisionscore = 0; | 2062 | d.Matrix3 mat = new d.Matrix3(); |
1883 | m_interpenetrationcount = 0; | 2063 | d.Quaternion quat = new d.Quaternion(); |
1884 | m_disabled = false; | 2064 | |
2065 | d.Mass tmpdmass = new d.Mass { }; | ||
2066 | d.Mass objdmass = new d.Mass { }; | ||
2067 | d.Vector3 dobjpos; | ||
2068 | d.Vector3 thispos; | ||
2069 | |||
2070 | d.BodyGetMass(Body, out objdmass); | ||
2071 | |||
2072 | // get prim own inertia in its local frame | ||
2073 | tmpdmass = primdMass; | ||
2074 | mat = d.GeomGetOffsetRotation(prim_geom); | ||
2075 | d.MassRotate(ref tmpdmass, ref mat); | ||
2076 | // transform to object frame | ||
2077 | thispos = d.GeomGetOffsetPosition(prim_geom); | ||
2078 | d.MassTranslate(ref tmpdmass, | ||
2079 | thispos.X, | ||
2080 | thispos.Y, | ||
2081 | thispos.Z); | ||
2082 | |||
2083 | // subtract current prim inertia from object | ||
2084 | DMassSubPartFromObj(ref tmpdmass, ref objdmass); | ||
2085 | |||
2086 | // update to new orientation | ||
2087 | _orientation = newrot; | ||
2088 | quat.X = newrot.X; | ||
2089 | quat.Y = newrot.Y; | ||
2090 | quat.Z = newrot.Z; | ||
2091 | quat.W = newrot.W; | ||
2092 | d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat); | ||
2093 | |||
2094 | tmpdmass = primdMass; | ||
2095 | mat = d.GeomGetOffsetRotation(prim_geom); | ||
2096 | d.MassRotate(ref tmpdmass, ref mat); | ||
2097 | d.MassTranslate(ref tmpdmass, | ||
2098 | thispos.X, | ||
2099 | thispos.Y, | ||
2100 | thispos.Z); | ||
2101 | |||
2102 | d.MassAdd(ref objdmass, ref tmpdmass); | ||
2103 | |||
2104 | // fix all positions | ||
2105 | IntPtr g = d.BodyGetFirstGeom(Body); | ||
2106 | while (g != IntPtr.Zero) | ||
2107 | { | ||
2108 | thispos = d.GeomGetOffsetPosition(g); | ||
2109 | thispos.X -= objdmass.c.X; | ||
2110 | thispos.Y -= objdmass.c.Y; | ||
2111 | thispos.Z -= objdmass.c.Z; | ||
2112 | d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); | ||
2113 | g = d.dBodyGetNextGeom(g); | ||
2114 | } | ||
2115 | |||
2116 | d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos); | ||
2117 | // get current object position and rotation | ||
2118 | dobjpos = d.BodyGetPosition(Body); | ||
2119 | |||
2120 | d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); | ||
2121 | d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body | ||
2122 | d.BodySetMass(Body, ref objdmass); | ||
2123 | _mass = objdmass.mass; | ||
1885 | } | 2124 | } |
1886 | 2125 | ||
1887 | /// <summary> | 2126 | |
1888 | /// Change prim in response to a disable taint. | 2127 | #region Mass Calculation |
1889 | /// </summary> | 2128 | |
1890 | private void changedisable() | 2129 | private void UpdatePrimBodyData() |
1891 | { | 2130 | { |
1892 | m_disabled = true; | 2131 | primMass = m_density * primVolume; |
1893 | if (Body != IntPtr.Zero) | 2132 | |
2133 | if (primMass <= 0) | ||
2134 | primMass = 0.0001f;//ckrinke: Mass must be greater then zero. | ||
2135 | if (primMass > _parent_scene.maximumMassObject) | ||
2136 | primMass = _parent_scene.maximumMassObject; | ||
2137 | |||
2138 | _mass = primMass; // just in case | ||
2139 | |||
2140 | d.MassSetBoxTotal(out primdMass, primMass, m_OBB.X, m_OBB.Y, m_OBB.Z); | ||
2141 | |||
2142 | d.MassTranslate(ref primdMass, | ||
2143 | m_OBBOffset.X, | ||
2144 | m_OBBOffset.Y, | ||
2145 | m_OBBOffset.Z); | ||
2146 | |||
2147 | primOOBradiusSQ = m_OBB.LengthSquared(); | ||
2148 | |||
2149 | if (_triMeshData != IntPtr.Zero) | ||
1894 | { | 2150 | { |
1895 | d.BodyDisable(Body); | 2151 | float pc = m_physCost; |
1896 | Body = IntPtr.Zero; | 2152 | float psf = primOOBradiusSQ; |
2153 | psf *= 1.33f * .2f; | ||
2154 | pc *= psf; | ||
2155 | if (pc < 0.1f) | ||
2156 | pc = 0.1f; | ||
2157 | |||
2158 | m_physCost = pc; | ||
1897 | } | 2159 | } |
2160 | else | ||
2161 | m_physCost = 0.1f; | ||
1898 | 2162 | ||
1899 | m_taintdisable = false; | 2163 | m_streamCost = 1.0f; |
1900 | } | 2164 | } |
1901 | 2165 | ||
2166 | #endregion | ||
2167 | |||
2168 | |||
1902 | /// <summary> | 2169 | /// <summary> |
1903 | /// Change prim in response to a physics status taint | 2170 | /// Add a child prim to this parent prim. |
1904 | /// </summary> | 2171 | /// </summary> |
1905 | private void changePhysicsStatus() | 2172 | /// <param name="prim">Child prim</param> |
2173 | // I'm the parent | ||
2174 | // prim is the child | ||
2175 | public void ParentPrim(OdePrim prim) | ||
1906 | { | 2176 | { |
1907 | if (IsPhysical) | 2177 | //Console.WriteLine("ParentPrim " + m_primName); |
2178 | if (this.m_localID != prim.m_localID) | ||
1908 | { | 2179 | { |
1909 | if (Body == IntPtr.Zero) | 2180 | DestroyBody(); // for now we need to rebuil entire object on link change |
2181 | |||
2182 | lock (childrenPrim) | ||
1910 | { | 2183 | { |
1911 | if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) | 2184 | // adopt the prim |
1912 | { | 2185 | if (!childrenPrim.Contains(prim)) |
1913 | changeshape(); | 2186 | childrenPrim.Add(prim); |
1914 | } | 2187 | |
1915 | else | 2188 | // see if this prim has kids and adopt them also |
2189 | // should not happen for now | ||
2190 | foreach (OdePrim prm in prim.childrenPrim) | ||
1916 | { | 2191 | { |
1917 | enableBody(); | 2192 | if (!childrenPrim.Contains(prm)) |
2193 | { | ||
2194 | if (prm.Body != IntPtr.Zero) | ||
2195 | { | ||
2196 | if (prm.prim_geom != IntPtr.Zero) | ||
2197 | d.GeomSetBody(prm.prim_geom, IntPtr.Zero); | ||
2198 | if (prm.Body != prim.Body) | ||
2199 | prm.DestroyBody(); // don't loose bodies around | ||
2200 | prm.Body = IntPtr.Zero; | ||
2201 | } | ||
2202 | |||
2203 | childrenPrim.Add(prm); | ||
2204 | prm._parent = this; | ||
2205 | } | ||
1918 | } | 2206 | } |
1919 | } | 2207 | } |
2208 | //Remove old children from the prim | ||
2209 | prim.childrenPrim.Clear(); | ||
2210 | |||
2211 | if (prim.Body != IntPtr.Zero) | ||
2212 | { | ||
2213 | if (prim.prim_geom != IntPtr.Zero) | ||
2214 | d.GeomSetBody(prim.prim_geom, IntPtr.Zero); | ||
2215 | prim.DestroyBody(); // don't loose bodies around | ||
2216 | prim.Body = IntPtr.Zero; | ||
2217 | } | ||
2218 | |||
2219 | prim.childPrim = true; | ||
2220 | prim._parent = this; | ||
2221 | |||
2222 | MakeBody(); // full nasty reconstruction | ||
1920 | } | 2223 | } |
1921 | else | 2224 | } |
2225 | |||
2226 | private void UpdateChildsfromgeom() | ||
2227 | { | ||
2228 | if (childrenPrim.Count > 0) | ||
1922 | { | 2229 | { |
1923 | if (Body != IntPtr.Zero) | 2230 | foreach (OdePrim prm in childrenPrim) |
1924 | { | 2231 | prm.UpdateDataFromGeom(); |
1925 | if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) | 2232 | } |
1926 | { | 2233 | } |
1927 | RemoveGeom(); | ||
1928 | 2234 | ||
1929 | //Console.WriteLine("changePhysicsStatus for " + Name); | 2235 | private void UpdateDataFromGeom() |
1930 | changeadd(); | 2236 | { |
1931 | } | 2237 | if (prim_geom != IntPtr.Zero) |
2238 | { | ||
2239 | d.Quaternion qtmp; | ||
2240 | d.GeomCopyQuaternion(prim_geom, out qtmp); | ||
2241 | _orientation.X = qtmp.X; | ||
2242 | _orientation.Y = qtmp.Y; | ||
2243 | _orientation.Z = qtmp.Z; | ||
2244 | _orientation.W = qtmp.W; | ||
2245 | /* | ||
2246 | // Debug | ||
2247 | float qlen = _orientation.Length(); | ||
2248 | if (qlen > 1.01f || qlen < 0.99) | ||
2249 | m_log.WarnFormat("[PHYSICS]: Got nonnorm quaternion from geom in Object {0} norm {1}", Name, qlen); | ||
2250 | // | ||
2251 | */ | ||
2252 | _orientation.Normalize(); | ||
2253 | |||
2254 | d.Vector3 lpos = d.GeomGetPosition(prim_geom); | ||
2255 | _position.X = lpos.X; | ||
2256 | _position.Y = lpos.Y; | ||
2257 | _position.Z = lpos.Z; | ||
2258 | } | ||
2259 | } | ||
2260 | |||
2261 | private void ChildDelink(OdePrim odePrim, bool remakebodies) | ||
2262 | { | ||
2263 | // Okay, we have a delinked child.. destroy all body and remake | ||
2264 | if (odePrim != this && !childrenPrim.Contains(odePrim)) | ||
2265 | return; | ||
2266 | |||
2267 | DestroyBody(); | ||
1932 | 2268 | ||
1933 | if (childPrim) | 2269 | if (odePrim == this) // delinking the root prim |
2270 | { | ||
2271 | OdePrim newroot = null; | ||
2272 | lock (childrenPrim) | ||
2273 | { | ||
2274 | if (childrenPrim.Count > 0) | ||
1934 | { | 2275 | { |
1935 | if (_parent != null) | 2276 | newroot = childrenPrim[0]; |
2277 | childrenPrim.RemoveAt(0); | ||
2278 | foreach (OdePrim prm in childrenPrim) | ||
1936 | { | 2279 | { |
1937 | OdePrim parent = (OdePrim)_parent; | 2280 | newroot.childrenPrim.Add(prm); |
1938 | parent.ChildDelink(this); | ||
1939 | } | 2281 | } |
2282 | childrenPrim.Clear(); | ||
1940 | } | 2283 | } |
1941 | else | 2284 | if (newroot != null) |
1942 | { | 2285 | { |
1943 | disableBody(); | 2286 | newroot.childPrim = false; |
2287 | newroot._parent = null; | ||
2288 | if (remakebodies) | ||
2289 | newroot.MakeBody(); | ||
1944 | } | 2290 | } |
1945 | } | 2291 | } |
1946 | } | 2292 | } |
1947 | 2293 | ||
1948 | changeSelectedStatus(); | 2294 | else |
1949 | 2295 | { | |
1950 | resetCollisionAccounting(); | 2296 | lock (childrenPrim) |
1951 | m_taintPhysics = IsPhysical; | 2297 | { |
2298 | childrenPrim.Remove(odePrim); | ||
2299 | odePrim.childPrim = false; | ||
2300 | odePrim._parent = null; | ||
2301 | // odePrim.UpdateDataFromGeom(); | ||
2302 | if (remakebodies) | ||
2303 | odePrim.MakeBody(); | ||
2304 | } | ||
2305 | } | ||
2306 | if (remakebodies) | ||
2307 | MakeBody(); | ||
1952 | } | 2308 | } |
1953 | 2309 | ||
1954 | /// <summary> | 2310 | protected void ChildRemove(OdePrim odePrim, bool reMakeBody) |
1955 | /// Change prim in response to a size taint. | ||
1956 | /// </summary> | ||
1957 | private void changesize() | ||
1958 | { | 2311 | { |
1959 | #if SPAM | 2312 | // Okay, we have a delinked child.. destroy all body and remake |
1960 | m_log.DebugFormat("[ODE PRIM]: Called changesize"); | 2313 | if (odePrim != this && !childrenPrim.Contains(odePrim)) |
1961 | #endif | 2314 | return; |
1962 | 2315 | ||
1963 | if (_size.X <= 0) _size.X = 0.01f; | 2316 | DestroyBody(); |
1964 | if (_size.Y <= 0) _size.Y = 0.01f; | ||
1965 | if (_size.Z <= 0) _size.Z = 0.01f; | ||
1966 | 2317 | ||
1967 | //kill body to rebuild | 2318 | if (odePrim == this) |
1968 | if (IsPhysical && Body != IntPtr.Zero) | ||
1969 | { | 2319 | { |
1970 | if (childPrim) | 2320 | OdePrim newroot = null; |
2321 | lock (childrenPrim) | ||
1971 | { | 2322 | { |
1972 | if (_parent != null) | 2323 | if (childrenPrim.Count > 0) |
1973 | { | 2324 | { |
1974 | OdePrim parent = (OdePrim)_parent; | 2325 | newroot = childrenPrim[0]; |
1975 | parent.ChildDelink(this); | 2326 | childrenPrim.RemoveAt(0); |
2327 | foreach (OdePrim prm in childrenPrim) | ||
2328 | { | ||
2329 | newroot.childrenPrim.Add(prm); | ||
2330 | } | ||
2331 | childrenPrim.Clear(); | ||
2332 | } | ||
2333 | if (newroot != null) | ||
2334 | { | ||
2335 | newroot.childPrim = false; | ||
2336 | newroot._parent = null; | ||
2337 | newroot.MakeBody(); | ||
1976 | } | 2338 | } |
1977 | } | 2339 | } |
1978 | else | 2340 | if (reMakeBody) |
1979 | { | 2341 | MakeBody(); |
1980 | disableBody(); | 2342 | return; |
1981 | } | ||
1982 | } | 2343 | } |
1983 | 2344 | else | |
1984 | if (d.SpaceQuery(m_targetSpace, prim_geom)) | ||
1985 | { | 2345 | { |
1986 | // _parent_scene.waitForSpaceUnlock(m_targetSpace); | 2346 | lock (childrenPrim) |
1987 | d.SpaceRemove(m_targetSpace, prim_geom); | 2347 | { |
2348 | childrenPrim.Remove(odePrim); | ||
2349 | odePrim.childPrim = false; | ||
2350 | odePrim._parent = null; | ||
2351 | if (reMakeBody) | ||
2352 | odePrim.MakeBody(); | ||
2353 | } | ||
1988 | } | 2354 | } |
2355 | MakeBody(); | ||
2356 | } | ||
1989 | 2357 | ||
1990 | RemoveGeom(); | 2358 | #region changes |
1991 | |||
1992 | // we don't need to do space calculation because the client sends a position update also. | ||
1993 | 2359 | ||
1994 | IMesh mesh = null; | 2360 | private void changeadd() |
2361 | { | ||
2362 | } | ||
1995 | 2363 | ||
1996 | // Construction of new prim | 2364 | private void changeAngularLock(Vector3 newLock) |
1997 | if (_parent_scene.needsMeshing(_pbs)) | 2365 | { |
2366 | // do we have a Physical object? | ||
2367 | if (Body != IntPtr.Zero) | ||
1998 | { | 2368 | { |
1999 | float meshlod = _parent_scene.meshSculptLOD; | 2369 | //Check that we have a Parent |
2000 | 2370 | //If we have a parent then we're not authorative here | |
2001 | if (IsPhysical) | 2371 | if (_parent == null) |
2002 | meshlod = _parent_scene.MeshSculptphysicalLOD; | ||
2003 | // Don't need to re-enable body.. it's done in SetMesh | ||
2004 | |||
2005 | if (_parent_scene.needsMeshing(_pbs)) | ||
2006 | { | 2372 | { |
2007 | mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical); | 2373 | if (!newLock.ApproxEquals(Vector3.One, 0f)) |
2008 | if (mesh == null) | 2374 | { |
2009 | CheckMeshAsset(); | 2375 | createAMotor(newLock); |
2376 | } | ||
2377 | else | ||
2378 | { | ||
2379 | if (Amotor != IntPtr.Zero) | ||
2380 | { | ||
2381 | d.JointDestroy(Amotor); | ||
2382 | Amotor = IntPtr.Zero; | ||
2383 | } | ||
2384 | } | ||
2010 | } | 2385 | } |
2011 | |||
2012 | } | 2386 | } |
2387 | // Store this for later in case we get turned into a separate body | ||
2388 | m_angularlock = newLock; | ||
2389 | } | ||
2013 | 2390 | ||
2014 | CreateGeom(m_targetSpace, mesh); | 2391 | private void changeLink(OdePrim NewParent) |
2015 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); | 2392 | { |
2016 | d.Quaternion myrot = new d.Quaternion(); | 2393 | if (_parent == null && NewParent != null) |
2017 | myrot.X = _orientation.X; | ||
2018 | myrot.Y = _orientation.Y; | ||
2019 | myrot.Z = _orientation.Z; | ||
2020 | myrot.W = _orientation.W; | ||
2021 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2022 | |||
2023 | //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z); | ||
2024 | if (IsPhysical && Body == IntPtr.Zero && !childPrim) | ||
2025 | { | 2394 | { |
2026 | // Re creates body on size. | 2395 | NewParent.ParentPrim(this); |
2027 | // EnableBody also does setMass() | ||
2028 | enableBody(); | ||
2029 | d.BodyEnable(Body); | ||
2030 | } | 2396 | } |
2031 | 2397 | else if (_parent != null) | |
2032 | changeSelectedStatus(); | ||
2033 | |||
2034 | if (childPrim) | ||
2035 | { | 2398 | { |
2036 | if (_parent is OdePrim) | 2399 | if (_parent is OdePrim) |
2037 | { | 2400 | { |
2038 | OdePrim parent = (OdePrim)_parent; | 2401 | if (NewParent != _parent) |
2039 | parent.ChildSetGeom(this); | 2402 | { |
2403 | (_parent as OdePrim).ChildDelink(this, false); // for now... | ||
2404 | childPrim = false; | ||
2405 | |||
2406 | if (NewParent != null) | ||
2407 | { | ||
2408 | NewParent.ParentPrim(this); | ||
2409 | } | ||
2410 | } | ||
2040 | } | 2411 | } |
2041 | } | 2412 | } |
2042 | resetCollisionAccounting(); | 2413 | _parent = NewParent; |
2043 | m_taintsize = _size; | ||
2044 | } | 2414 | } |
2045 | 2415 | ||
2046 | /// <summary> | ||
2047 | /// Change prim in response to a float on water taint. | ||
2048 | /// </summary> | ||
2049 | /// <param name="timestep"></param> | ||
2050 | private void changefloatonwater() | ||
2051 | { | ||
2052 | m_collidesWater = m_taintCollidesWater; | ||
2053 | 2416 | ||
2054 | if (m_collidesWater) | 2417 | private void Stop() |
2418 | { | ||
2419 | if (!childPrim) | ||
2055 | { | 2420 | { |
2056 | m_collisionFlags |= CollisionCategories.Water; | 2421 | m_force = Vector3.Zero; |
2422 | m_forceacc = Vector3.Zero; | ||
2423 | m_angularForceacc = Vector3.Zero; | ||
2424 | _torque = Vector3.Zero; | ||
2425 | _velocity = Vector3.Zero; | ||
2426 | _acceleration = Vector3.Zero; | ||
2427 | m_rotationalVelocity = Vector3.Zero; | ||
2428 | _target_velocity = Vector3.Zero; | ||
2429 | if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) | ||
2430 | m_vehicle.Stop(); | ||
2431 | |||
2432 | _zeroFlag = false; | ||
2433 | base.RequestPhysicsterseUpdate(); | ||
2057 | } | 2434 | } |
2058 | else | 2435 | |
2436 | if (Body != IntPtr.Zero) | ||
2059 | { | 2437 | { |
2060 | m_collisionFlags &= ~CollisionCategories.Water; | 2438 | d.BodySetForce(Body, 0f, 0f, 0f); |
2439 | d.BodySetTorque(Body, 0f, 0f, 0f); | ||
2440 | d.BodySetLinearVel(Body, 0f, 0f, 0f); | ||
2441 | d.BodySetAngularVel(Body, 0f, 0f, 0f); | ||
2061 | } | 2442 | } |
2062 | |||
2063 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
2064 | } | 2443 | } |
2065 | 2444 | ||
2066 | /// <summary> | 2445 | private void changePhantomStatus(bool newval) |
2067 | /// Change prim in response to a shape taint. | ||
2068 | /// </summary> | ||
2069 | private void changeshape() | ||
2070 | { | 2446 | { |
2071 | m_taintshape = false; | 2447 | m_isphantom = newval; |
2072 | |||
2073 | // Cleanup of old prim geometry and Bodies | ||
2074 | if (IsPhysical && Body != IntPtr.Zero) | ||
2075 | { | ||
2076 | if (childPrim) | ||
2077 | { | ||
2078 | if (_parent != null) | ||
2079 | { | ||
2080 | OdePrim parent = (OdePrim)_parent; | ||
2081 | parent.ChildDelink(this); | ||
2082 | } | ||
2083 | } | ||
2084 | else | ||
2085 | { | ||
2086 | disableBody(); | ||
2087 | } | ||
2088 | } | ||
2089 | 2448 | ||
2090 | RemoveGeom(); | 2449 | UpdateCollisionCatFlags(); |
2091 | 2450 | ApplyCollisionCatFlags(); | |
2092 | // we don't need to do space calculation because the client sends a position update also. | 2451 | } |
2093 | if (_size.X <= 0) _size.X = 0.01f; | ||
2094 | if (_size.Y <= 0) _size.Y = 0.01f; | ||
2095 | if (_size.Z <= 0) _size.Z = 0.01f; | ||
2096 | // Construction of new prim | ||
2097 | 2452 | ||
2098 | IMesh mesh = null; | 2453 | /* not in use |
2454 | internal void ChildSelectedChange(bool childSelect) | ||
2455 | { | ||
2456 | if(childPrim) | ||
2457 | return; | ||
2099 | 2458 | ||
2459 | if (childSelect == m_isSelected) | ||
2460 | return; | ||
2100 | 2461 | ||
2101 | if (_parent_scene.needsMeshing(_pbs)) | 2462 | if (childSelect) |
2102 | { | 2463 | { |
2103 | // Don't need to re-enable body.. it's done in CreateMesh | 2464 | DoSelectedStatus(true); |
2104 | float meshlod = _parent_scene.meshSculptLOD; | ||
2105 | |||
2106 | if (IsPhysical) | ||
2107 | meshlod = _parent_scene.MeshSculptphysicalLOD; | ||
2108 | |||
2109 | // createmesh returns null when it doesn't mesh. | ||
2110 | mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical); | ||
2111 | if (mesh == null) | ||
2112 | CheckMeshAsset(); | ||
2113 | } | 2465 | } |
2114 | 2466 | ||
2115 | CreateGeom(m_targetSpace, mesh); | 2467 | else |
2116 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); | ||
2117 | d.Quaternion myrot = new d.Quaternion(); | ||
2118 | //myrot.W = _orientation.w; | ||
2119 | myrot.W = _orientation.W; | ||
2120 | myrot.X = _orientation.X; | ||
2121 | myrot.Y = _orientation.Y; | ||
2122 | myrot.Z = _orientation.Z; | ||
2123 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2124 | |||
2125 | //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z); | ||
2126 | if (IsPhysical && Body == IntPtr.Zero) | ||
2127 | { | 2468 | { |
2128 | // Re creates body on size. | 2469 | foreach (OdePrim prm in childrenPrim) |
2129 | // EnableBody also does setMass() | ||
2130 | enableBody(); | ||
2131 | if (Body != IntPtr.Zero) | ||
2132 | { | 2470 | { |
2133 | d.BodyEnable(Body); | 2471 | if (prm.m_isSelected) |
2472 | return; | ||
2134 | } | 2473 | } |
2474 | DoSelectedStatus(false); | ||
2135 | } | 2475 | } |
2476 | } | ||
2477 | */ | ||
2478 | private void changeSelectedStatus(bool newval) | ||
2479 | { | ||
2480 | if (m_lastdoneSelected == newval) | ||
2481 | return; | ||
2136 | 2482 | ||
2137 | changeSelectedStatus(); | 2483 | m_lastdoneSelected = newval; |
2484 | DoSelectedStatus(newval); | ||
2485 | } | ||
2138 | 2486 | ||
2139 | if (childPrim) | 2487 | private void CheckDelaySelect() |
2488 | { | ||
2489 | if (m_delaySelect) | ||
2140 | { | 2490 | { |
2141 | if (_parent is OdePrim) | 2491 | DoSelectedStatus(m_isSelected); |
2142 | { | ||
2143 | OdePrim parent = (OdePrim)_parent; | ||
2144 | parent.ChildSetGeom(this); | ||
2145 | } | ||
2146 | } | 2492 | } |
2147 | |||
2148 | resetCollisionAccounting(); | ||
2149 | // m_taintshape = false; | ||
2150 | } | 2493 | } |
2151 | 2494 | ||
2152 | /// <summary> | 2495 | private void DoSelectedStatus(bool newval) |
2153 | /// Change prim in response to an add force taint. | ||
2154 | /// </summary> | ||
2155 | private void changeAddForce() | ||
2156 | { | 2496 | { |
2157 | if (!m_isSelected) | 2497 | m_isSelected = newval; |
2498 | Stop(); | ||
2499 | |||
2500 | if (newval) | ||
2158 | { | 2501 | { |
2159 | lock (m_forcelist) | 2502 | if (!childPrim && Body != IntPtr.Zero) |
2503 | d.BodyDisable(Body); | ||
2504 | |||
2505 | if (m_delaySelect || m_isphysical) | ||
2160 | { | 2506 | { |
2161 | //m_log.Info("[PHYSICS]: dequeing forcelist"); | 2507 | m_collisionCategories = CollisionCategories.Selected; |
2162 | if (IsPhysical) | 2508 | m_collisionFlags = 0; |
2509 | |||
2510 | if (!childPrim) | ||
2163 | { | 2511 | { |
2164 | Vector3 iforce = Vector3.Zero; | 2512 | foreach (OdePrim prm in childrenPrim) |
2165 | int i = 0; | ||
2166 | try | ||
2167 | { | 2513 | { |
2168 | for (i = 0; i < m_forcelist.Count; i++) | 2514 | prm.m_collisionCategories = m_collisionCategories; |
2515 | prm.m_collisionFlags = m_collisionFlags; | ||
2516 | |||
2517 | if (prm.prim_geom != null) | ||
2169 | { | 2518 | { |
2170 | 2519 | ||
2171 | iforce = iforce + (m_forcelist[i] * 100); | 2520 | if (prm.m_NoColide) |
2521 | { | ||
2522 | d.GeomSetCategoryBits(prm.prim_geom, 0); | ||
2523 | d.GeomSetCollideBits(prm.prim_geom, 0); | ||
2524 | } | ||
2525 | else | ||
2526 | { | ||
2527 | d.GeomSetCategoryBits(prm.prim_geom, (uint)m_collisionCategories); | ||
2528 | d.GeomSetCollideBits(prm.prim_geom, (uint)m_collisionFlags); | ||
2529 | } | ||
2172 | } | 2530 | } |
2531 | prm.m_delaySelect = false; | ||
2173 | } | 2532 | } |
2174 | catch (IndexOutOfRangeException) | 2533 | } |
2534 | // else if (_parent != null) | ||
2535 | // ((OdePrim)_parent).ChildSelectedChange(true); | ||
2536 | |||
2537 | |||
2538 | if (prim_geom != null) | ||
2539 | { | ||
2540 | if (m_NoColide) | ||
2175 | { | 2541 | { |
2176 | m_forcelist = new List<Vector3>(); | 2542 | d.GeomSetCategoryBits(prim_geom, 0); |
2177 | m_collisionscore = 0; | 2543 | d.GeomSetCollideBits(prim_geom, 0); |
2178 | m_interpenetrationcount = 0; | 2544 | if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) |
2179 | m_taintforce = false; | 2545 | { |
2180 | return; | 2546 | d.GeomSetCategoryBits(collide_geom, 0); |
2547 | d.GeomSetCollideBits(collide_geom, 0); | ||
2548 | } | ||
2549 | |||
2181 | } | 2550 | } |
2182 | catch (ArgumentOutOfRangeException) | 2551 | else |
2183 | { | 2552 | { |
2184 | m_forcelist = new List<Vector3>(); | 2553 | d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); |
2185 | m_collisionscore = 0; | 2554 | d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); |
2186 | m_interpenetrationcount = 0; | 2555 | if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) |
2187 | m_taintforce = false; | 2556 | { |
2188 | return; | 2557 | d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories); |
2558 | d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags); | ||
2559 | } | ||
2189 | } | 2560 | } |
2190 | d.BodyEnable(Body); | ||
2191 | d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z); | ||
2192 | } | 2561 | } |
2193 | m_forcelist.Clear(); | 2562 | |
2563 | m_delaySelect = false; | ||
2564 | } | ||
2565 | else if(!m_isphysical) | ||
2566 | { | ||
2567 | m_delaySelect = true; | ||
2568 | } | ||
2569 | } | ||
2570 | else | ||
2571 | { | ||
2572 | if (!childPrim) | ||
2573 | { | ||
2574 | if (Body != IntPtr.Zero && !m_disabled) | ||
2575 | d.BodyEnable(Body); | ||
2194 | } | 2576 | } |
2577 | // else if (_parent != null) | ||
2578 | // ((OdePrim)_parent).ChildSelectedChange(false); | ||
2195 | 2579 | ||
2196 | m_collisionscore = 0; | 2580 | UpdateCollisionCatFlags(); |
2197 | m_interpenetrationcount = 0; | 2581 | ApplyCollisionCatFlags(); |
2582 | |||
2583 | m_delaySelect = false; | ||
2198 | } | 2584 | } |
2199 | 2585 | ||
2200 | m_taintforce = false; | 2586 | resetCollisionAccounting(); |
2201 | } | 2587 | } |
2202 | 2588 | ||
2203 | /// <summary> | 2589 | private void changePosition(Vector3 newPos) |
2204 | /// Change prim in response to a torque taint. | ||
2205 | /// </summary> | ||
2206 | private void changeSetTorque() | ||
2207 | { | 2590 | { |
2208 | if (!m_isSelected) | 2591 | CheckDelaySelect(); |
2592 | if (m_isphysical) | ||
2209 | { | 2593 | { |
2210 | if (IsPhysical && Body != IntPtr.Zero) | 2594 | if (childPrim) // inertia is messed, must rebuild |
2595 | { | ||
2596 | if (m_building) | ||
2597 | { | ||
2598 | _position = newPos; | ||
2599 | } | ||
2600 | |||
2601 | else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero) | ||
2602 | { | ||
2603 | FixInertia(newPos); | ||
2604 | if (!d.BodyIsEnabled(Body)) | ||
2605 | d.BodyEnable(Body); | ||
2606 | } | ||
2607 | } | ||
2608 | else | ||
2211 | { | 2609 | { |
2212 | d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z); | 2610 | if (_position != newPos) |
2611 | { | ||
2612 | d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); | ||
2613 | _position = newPos; | ||
2614 | } | ||
2615 | if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) | ||
2616 | d.BodyEnable(Body); | ||
2213 | } | 2617 | } |
2214 | } | 2618 | } |
2619 | else | ||
2620 | { | ||
2621 | if (prim_geom != IntPtr.Zero) | ||
2622 | { | ||
2623 | if (newPos != _position) | ||
2624 | { | ||
2625 | d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); | ||
2626 | _position = newPos; | ||
2215 | 2627 | ||
2216 | m_taintTorque = Vector3.Zero; | 2628 | m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); |
2629 | } | ||
2630 | } | ||
2631 | } | ||
2632 | givefakepos--; | ||
2633 | if (givefakepos < 0) | ||
2634 | givefakepos = 0; | ||
2635 | // changeSelectedStatus(); | ||
2636 | resetCollisionAccounting(); | ||
2217 | } | 2637 | } |
2218 | 2638 | ||
2219 | /// <summary> | 2639 | private void changeOrientation(Quaternion newOri) |
2220 | /// Change prim in response to an angular force taint. | ||
2221 | /// </summary> | ||
2222 | private void changeAddAngularForce() | ||
2223 | { | 2640 | { |
2224 | if (!m_isSelected) | 2641 | CheckDelaySelect(); |
2642 | if (m_isphysical) | ||
2225 | { | 2643 | { |
2226 | lock (m_angularforcelist) | 2644 | if (childPrim) // inertia is messed, must rebuild |
2227 | { | 2645 | { |
2228 | //m_log.Info("[PHYSICS]: dequeing forcelist"); | 2646 | if (m_building) |
2229 | if (IsPhysical) | ||
2230 | { | 2647 | { |
2231 | Vector3 iforce = Vector3.Zero; | 2648 | _orientation = newOri; |
2232 | for (int i = 0; i < m_angularforcelist.Count; i++) | 2649 | } |
2233 | { | 2650 | /* |
2234 | iforce = iforce + (m_angularforcelist[i] * 100); | 2651 | else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero) |
2235 | } | 2652 | { |
2653 | FixInertia(_position, newOri); | ||
2654 | if (!d.BodyIsEnabled(Body)) | ||
2655 | d.BodyEnable(Body); | ||
2656 | } | ||
2657 | */ | ||
2658 | } | ||
2659 | else | ||
2660 | { | ||
2661 | if (newOri != _orientation) | ||
2662 | { | ||
2663 | d.Quaternion myrot = new d.Quaternion(); | ||
2664 | myrot.X = newOri.X; | ||
2665 | myrot.Y = newOri.Y; | ||
2666 | myrot.Z = newOri.Z; | ||
2667 | myrot.W = newOri.W; | ||
2668 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2669 | _orientation = newOri; | ||
2670 | if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) | ||
2671 | createAMotor(m_angularlock); | ||
2672 | } | ||
2673 | if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) | ||
2236 | d.BodyEnable(Body); | 2674 | d.BodyEnable(Body); |
2237 | d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z); | 2675 | } |
2238 | 2676 | } | |
2677 | else | ||
2678 | { | ||
2679 | if (prim_geom != IntPtr.Zero) | ||
2680 | { | ||
2681 | if (newOri != _orientation) | ||
2682 | { | ||
2683 | d.Quaternion myrot = new d.Quaternion(); | ||
2684 | myrot.X = newOri.X; | ||
2685 | myrot.Y = newOri.Y; | ||
2686 | myrot.Z = newOri.Z; | ||
2687 | myrot.W = newOri.W; | ||
2688 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2689 | _orientation = newOri; | ||
2239 | } | 2690 | } |
2240 | m_angularforcelist.Clear(); | ||
2241 | } | 2691 | } |
2242 | |||
2243 | m_collisionscore = 0; | ||
2244 | m_interpenetrationcount = 0; | ||
2245 | } | 2692 | } |
2246 | 2693 | givefakeori--; | |
2247 | m_taintaddangularforce = false; | 2694 | if (givefakeori < 0) |
2695 | givefakeori = 0; | ||
2696 | resetCollisionAccounting(); | ||
2248 | } | 2697 | } |
2249 | 2698 | ||
2250 | /// <summary> | 2699 | private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri) |
2251 | /// Change prim in response to a velocity taint. | ||
2252 | /// </summary> | ||
2253 | private void changevelocity() | ||
2254 | { | 2700 | { |
2255 | if (!m_isSelected) | 2701 | CheckDelaySelect(); |
2702 | if (m_isphysical) | ||
2256 | { | 2703 | { |
2257 | // Not sure exactly why this sleep is here, but from experimentation it appears to stop an avatar | 2704 | if (childPrim && m_building) // inertia is messed, must rebuild |
2258 | // walking through a default rez size prim if it keeps kicking it around - justincc. | ||
2259 | Thread.Sleep(20); | ||
2260 | |||
2261 | if (IsPhysical) | ||
2262 | { | 2705 | { |
2263 | if (Body != IntPtr.Zero) | 2706 | _position = newPos; |
2707 | _orientation = newOri; | ||
2708 | } | ||
2709 | else | ||
2710 | { | ||
2711 | if (newOri != _orientation) | ||
2712 | { | ||
2713 | d.Quaternion myrot = new d.Quaternion(); | ||
2714 | myrot.X = newOri.X; | ||
2715 | myrot.Y = newOri.Y; | ||
2716 | myrot.Z = newOri.Z; | ||
2717 | myrot.W = newOri.W; | ||
2718 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2719 | _orientation = newOri; | ||
2720 | if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) | ||
2721 | createAMotor(m_angularlock); | ||
2722 | } | ||
2723 | if (_position != newPos) | ||
2264 | { | 2724 | { |
2265 | d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z); | 2725 | d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); |
2726 | _position = newPos; | ||
2266 | } | 2727 | } |
2728 | if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) | ||
2729 | d.BodyEnable(Body); | ||
2267 | } | 2730 | } |
2268 | |||
2269 | //resetCollisionAccounting(); | ||
2270 | } | 2731 | } |
2732 | else | ||
2733 | { | ||
2734 | // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); | ||
2735 | // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); | ||
2271 | 2736 | ||
2272 | m_taintVelocity = Vector3.Zero; | 2737 | if (prim_geom != IntPtr.Zero) |
2273 | } | 2738 | { |
2739 | if (newOri != _orientation) | ||
2740 | { | ||
2741 | d.Quaternion myrot = new d.Quaternion(); | ||
2742 | myrot.X = newOri.X; | ||
2743 | myrot.Y = newOri.Y; | ||
2744 | myrot.Z = newOri.Z; | ||
2745 | myrot.W = newOri.W; | ||
2746 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2747 | _orientation = newOri; | ||
2748 | } | ||
2274 | 2749 | ||
2275 | internal void setPrimForRemoval() | 2750 | if (newPos != _position) |
2276 | { | 2751 | { |
2277 | m_taintremove = true; | 2752 | d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); |
2278 | } | 2753 | _position = newPos; |
2279 | 2754 | ||
2280 | public override bool Flying | 2755 | m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); |
2281 | { | 2756 | } |
2282 | // no flying prims for you | 2757 | } |
2283 | get { return false; } | 2758 | } |
2284 | set { } | 2759 | givefakepos--; |
2760 | if (givefakepos < 0) | ||
2761 | givefakepos = 0; | ||
2762 | givefakeori--; | ||
2763 | if (givefakeori < 0) | ||
2764 | givefakeori = 0; | ||
2765 | resetCollisionAccounting(); | ||
2285 | } | 2766 | } |
2286 | 2767 | ||
2287 | public override bool IsColliding | 2768 | private void changeDisable(bool disable) |
2288 | { | 2769 | { |
2289 | get { return iscolliding; } | 2770 | if (disable) |
2290 | set { iscolliding = value; } | 2771 | { |
2772 | if (!m_disabled) | ||
2773 | disableBodySoft(); | ||
2774 | } | ||
2775 | else | ||
2776 | { | ||
2777 | if (m_disabled) | ||
2778 | enableBodySoft(); | ||
2779 | } | ||
2291 | } | 2780 | } |
2292 | 2781 | ||
2293 | public override bool CollidingGround | 2782 | private void changePhysicsStatus(bool NewStatus) |
2294 | { | 2783 | { |
2295 | get { return false; } | 2784 | CheckDelaySelect(); |
2296 | set { return; } | ||
2297 | } | ||
2298 | 2785 | ||
2299 | public override bool CollidingObj | 2786 | m_isphysical = NewStatus; |
2300 | { | 2787 | |
2301 | get { return false; } | 2788 | if (!childPrim) |
2302 | set { return; } | 2789 | { |
2790 | if (NewStatus) | ||
2791 | { | ||
2792 | if (Body == IntPtr.Zero) | ||
2793 | MakeBody(); | ||
2794 | } | ||
2795 | else | ||
2796 | { | ||
2797 | if (Body != IntPtr.Zero) | ||
2798 | { | ||
2799 | DestroyBody(); | ||
2800 | } | ||
2801 | Stop(); | ||
2802 | } | ||
2803 | } | ||
2804 | |||
2805 | resetCollisionAccounting(); | ||
2303 | } | 2806 | } |
2304 | 2807 | ||
2305 | public override bool ThrottleUpdates | 2808 | private void changeSize(Vector3 newSize) |
2306 | { | 2809 | { |
2307 | get { return m_throttleUpdates; } | ||
2308 | set { m_throttleUpdates = value; } | ||
2309 | } | 2810 | } |
2310 | 2811 | ||
2311 | public override bool Stopped | 2812 | private void changeShape(PrimitiveBaseShape newShape) |
2312 | { | 2813 | { |
2313 | get { return _zeroFlag; } | ||
2314 | } | 2814 | } |
2315 | 2815 | ||
2316 | public override Vector3 Position | 2816 | private void changeAddPhysRep(ODEPhysRepData repData) |
2317 | { | 2817 | { |
2318 | get { return _position; } | 2818 | _size = repData.size; //?? |
2819 | _pbs = repData.pbs; | ||
2820 | m_shapetype = repData.shapetype; | ||
2821 | |||
2822 | m_mesh = repData.mesh; | ||
2823 | |||
2824 | m_assetID = repData.assetID; | ||
2825 | m_meshState = repData.meshState; | ||
2826 | |||
2827 | m_hasOBB = repData.hasOBB; | ||
2828 | m_OBBOffset = repData.OBBOffset; | ||
2829 | m_OBB = repData.OBB; | ||
2319 | 2830 | ||
2320 | set { _position = value; | 2831 | primVolume = repData.volume; |
2321 | //m_log.Info("[PHYSICS]: " + _position.ToString()); | 2832 | |
2833 | CreateGeom(); | ||
2834 | |||
2835 | if (prim_geom != IntPtr.Zero) | ||
2836 | { | ||
2837 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); | ||
2838 | d.Quaternion myrot = new d.Quaternion(); | ||
2839 | myrot.X = _orientation.X; | ||
2840 | myrot.Y = _orientation.Y; | ||
2841 | myrot.Z = _orientation.Z; | ||
2842 | myrot.W = _orientation.W; | ||
2843 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2844 | } | ||
2845 | |||
2846 | if (!m_isphysical) | ||
2847 | { | ||
2848 | SetInStaticSpace(this); | ||
2849 | UpdateCollisionCatFlags(); | ||
2850 | ApplyCollisionCatFlags(); | ||
2851 | } | ||
2852 | else | ||
2853 | MakeBody(); | ||
2854 | |||
2855 | if ((m_meshState & MeshState.NeedMask) != 0) | ||
2856 | { | ||
2857 | repData.size = _size; | ||
2858 | repData.pbs = _pbs; | ||
2859 | repData.shapetype = m_shapetype; | ||
2860 | _parent_scene.m_meshWorker.RequestMesh(repData); | ||
2322 | } | 2861 | } |
2323 | } | 2862 | } |
2324 | 2863 | ||
2325 | public override Vector3 Size | 2864 | private void changePhysRepData(ODEPhysRepData repData) |
2326 | { | 2865 | { |
2327 | get { return _size; } | 2866 | CheckDelaySelect(); |
2328 | set | 2867 | |
2868 | OdePrim parent = (OdePrim)_parent; | ||
2869 | |||
2870 | bool chp = childPrim; | ||
2871 | |||
2872 | if (chp) | ||
2329 | { | 2873 | { |
2330 | if (value.IsFinite()) | 2874 | if (parent != null) |
2331 | { | 2875 | { |
2332 | _size = value; | 2876 | parent.DestroyBody(); |
2333 | // m_log.DebugFormat("[PHYSICS]: Set size on {0} to {1}", Name, value); | ||
2334 | } | ||
2335 | else | ||
2336 | { | ||
2337 | m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name); | ||
2338 | } | 2877 | } |
2339 | } | 2878 | } |
2340 | } | 2879 | else |
2880 | { | ||
2881 | DestroyBody(); | ||
2882 | } | ||
2341 | 2883 | ||
2342 | public override float Mass | 2884 | RemoveGeom(); |
2343 | { | ||
2344 | get { return CalculateMass(); } | ||
2345 | } | ||
2346 | 2885 | ||
2347 | public override Vector3 Force | 2886 | _size = repData.size; |
2348 | { | 2887 | _pbs = repData.pbs; |
2349 | //get { return Vector3.Zero; } | 2888 | m_shapetype = repData.shapetype; |
2350 | get { return m_force; } | 2889 | |
2351 | set | 2890 | m_mesh = repData.mesh; |
2891 | |||
2892 | m_assetID = repData.assetID; | ||
2893 | m_meshState = repData.meshState; | ||
2894 | |||
2895 | m_hasOBB = repData.hasOBB; | ||
2896 | m_OBBOffset = repData.OBBOffset; | ||
2897 | m_OBB = repData.OBB; | ||
2898 | |||
2899 | primVolume = repData.volume; | ||
2900 | |||
2901 | CreateGeom(); | ||
2902 | |||
2903 | if (prim_geom != IntPtr.Zero) | ||
2352 | { | 2904 | { |
2353 | if (value.IsFinite()) | 2905 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); |
2906 | d.Quaternion myrot = new d.Quaternion(); | ||
2907 | myrot.X = _orientation.X; | ||
2908 | myrot.Y = _orientation.Y; | ||
2909 | myrot.Z = _orientation.Z; | ||
2910 | myrot.W = _orientation.W; | ||
2911 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2912 | } | ||
2913 | |||
2914 | if (m_isphysical) | ||
2915 | { | ||
2916 | if (chp) | ||
2354 | { | 2917 | { |
2355 | m_force = value; | 2918 | if (parent != null) |
2919 | { | ||
2920 | parent.MakeBody(); | ||
2921 | } | ||
2356 | } | 2922 | } |
2357 | else | 2923 | else |
2358 | { | 2924 | MakeBody(); |
2359 | m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name); | 2925 | } |
2360 | } | 2926 | else |
2927 | { | ||
2928 | SetInStaticSpace(this); | ||
2929 | UpdateCollisionCatFlags(); | ||
2930 | ApplyCollisionCatFlags(); | ||
2361 | } | 2931 | } |
2362 | } | ||
2363 | |||
2364 | public override int VehicleType | ||
2365 | { | ||
2366 | get { return (int)m_vehicle.Type; } | ||
2367 | set { m_vehicle.ProcessTypeChange((Vehicle)value); } | ||
2368 | } | ||
2369 | 2932 | ||
2370 | public override void VehicleFloatParam(int param, float value) | 2933 | resetCollisionAccounting(); |
2371 | { | ||
2372 | m_vehicle.ProcessFloatVehicleParam((Vehicle) param, value); | ||
2373 | } | ||
2374 | 2934 | ||
2375 | public override void VehicleVectorParam(int param, Vector3 value) | 2935 | if ((m_meshState & MeshState.NeedMask) != 0) |
2376 | { | 2936 | { |
2377 | m_vehicle.ProcessVectorVehicleParam((Vehicle) param, value); | 2937 | repData.size = _size; |
2938 | repData.pbs = _pbs; | ||
2939 | repData.shapetype = m_shapetype; | ||
2940 | _parent_scene.m_meshWorker.RequestMesh(repData); | ||
2941 | } | ||
2378 | } | 2942 | } |
2379 | 2943 | ||
2380 | public override void VehicleRotationParam(int param, Quaternion rotation) | 2944 | private void changeFloatOnWater(bool newval) |
2381 | { | 2945 | { |
2382 | m_vehicle.ProcessRotationVehicleParam((Vehicle) param, rotation); | 2946 | m_collidesWater = newval; |
2383 | } | ||
2384 | 2947 | ||
2385 | public override void VehicleFlags(int param, bool remove) | 2948 | UpdateCollisionCatFlags(); |
2386 | { | 2949 | ApplyCollisionCatFlags(); |
2387 | m_vehicle.ProcessVehicleFlags(param, remove); | ||
2388 | } | 2950 | } |
2389 | 2951 | ||
2390 | public override void SetVolumeDetect(int param) | 2952 | private void changeSetTorque(Vector3 newtorque) |
2391 | { | 2953 | { |
2392 | // We have to lock the scene here so that an entire simulate loop either uses volume detect for all | 2954 | if (!m_isSelected) |
2393 | // possible collisions with this prim or for none of them. | ||
2394 | lock (_parent_scene.OdeLock) | ||
2395 | { | 2955 | { |
2396 | m_isVolumeDetect = (param != 0); | 2956 | if (m_isphysical && Body != IntPtr.Zero) |
2957 | { | ||
2958 | if (m_disabled) | ||
2959 | enableBodySoft(); | ||
2960 | else if (!d.BodyIsEnabled(Body)) | ||
2961 | d.BodyEnable(Body); | ||
2962 | |||
2963 | } | ||
2964 | _torque = newtorque; | ||
2397 | } | 2965 | } |
2398 | } | 2966 | } |
2399 | 2967 | ||
2400 | public override Vector3 CenterOfMass | 2968 | private void changeForce(Vector3 force) |
2401 | { | 2969 | { |
2402 | get { return Vector3.Zero; } | 2970 | m_force = force; |
2971 | if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) | ||
2972 | d.BodyEnable(Body); | ||
2403 | } | 2973 | } |
2404 | 2974 | ||
2405 | public override Vector3 GeometricCenter | 2975 | private void changeAddForce(Vector3 theforce) |
2406 | { | 2976 | { |
2407 | get { return Vector3.Zero; } | 2977 | m_forceacc += theforce; |
2978 | if (!m_isSelected) | ||
2979 | { | ||
2980 | lock (this) | ||
2981 | { | ||
2982 | //m_log.Info("[PHYSICS]: dequeing forcelist"); | ||
2983 | if (m_isphysical && Body != IntPtr.Zero) | ||
2984 | { | ||
2985 | if (m_disabled) | ||
2986 | enableBodySoft(); | ||
2987 | else if (!d.BodyIsEnabled(Body)) | ||
2988 | d.BodyEnable(Body); | ||
2989 | } | ||
2990 | } | ||
2991 | m_collisionscore = 0; | ||
2992 | } | ||
2408 | } | 2993 | } |
2409 | 2994 | ||
2410 | public override PrimitiveBaseShape Shape | 2995 | // actually angular impulse |
2996 | private void changeAddAngularImpulse(Vector3 aimpulse) | ||
2411 | { | 2997 | { |
2412 | set | 2998 | m_angularForceacc += aimpulse * m_invTimeStep; |
2999 | if (!m_isSelected) | ||
2413 | { | 3000 | { |
2414 | _pbs = value; | 3001 | lock (this) |
2415 | m_assetFailed = false; | 3002 | { |
2416 | m_taintshape = true; | 3003 | if (m_isphysical && Body != IntPtr.Zero) |
3004 | { | ||
3005 | if (m_disabled) | ||
3006 | enableBodySoft(); | ||
3007 | else if (!d.BodyIsEnabled(Body)) | ||
3008 | d.BodyEnable(Body); | ||
3009 | } | ||
3010 | } | ||
3011 | m_collisionscore = 0; | ||
2417 | } | 3012 | } |
2418 | } | 3013 | } |
2419 | 3014 | ||
2420 | public override Vector3 Velocity | 3015 | private void changevelocity(Vector3 newVel) |
2421 | { | 3016 | { |
2422 | get | 3017 | float len = newVel.LengthSquared(); |
3018 | if (len > 100000.0f) // limit to 100m/s | ||
2423 | { | 3019 | { |
2424 | // Average previous velocity with the new one so | 3020 | len = 100.0f / (float)Math.Sqrt(len); |
2425 | // client object interpolation works a 'little' better | 3021 | newVel *= len; |
2426 | if (_zeroFlag) | ||
2427 | return Vector3.Zero; | ||
2428 | |||
2429 | Vector3 returnVelocity = Vector3.Zero; | ||
2430 | returnVelocity.X = (m_lastVelocity.X + _velocity.X) * 0.5f; // 0.5f is mathematically equiv to '/ 2' | ||
2431 | returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) * 0.5f; | ||
2432 | returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) * 0.5f; | ||
2433 | return returnVelocity; | ||
2434 | } | 3022 | } |
2435 | set | 3023 | |
3024 | if (!m_isSelected) | ||
2436 | { | 3025 | { |
2437 | if (value.IsFinite()) | 3026 | if (Body != IntPtr.Zero) |
2438 | { | 3027 | { |
2439 | _velocity = value; | 3028 | if (m_disabled) |
3029 | enableBodySoft(); | ||
3030 | else if (!d.BodyIsEnabled(Body)) | ||
3031 | d.BodyEnable(Body); | ||
2440 | 3032 | ||
2441 | m_taintVelocity = value; | 3033 | d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z); |
2442 | _parent_scene.AddPhysicsActorTaint(this); | ||
2443 | } | ||
2444 | else | ||
2445 | { | ||
2446 | m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name); | ||
2447 | } | 3034 | } |
2448 | 3035 | //resetCollisionAccounting(); | |
2449 | } | 3036 | } |
3037 | _velocity = newVel; | ||
2450 | } | 3038 | } |
2451 | 3039 | ||
2452 | public override Vector3 Torque | 3040 | private void changeangvelocity(Vector3 newAngVel) |
2453 | { | 3041 | { |
2454 | get | 3042 | float len = newAngVel.LengthSquared(); |
3043 | if (len > 144.0f) // limit to 12rad/s | ||
2455 | { | 3044 | { |
2456 | if (!IsPhysical || Body == IntPtr.Zero) | 3045 | len = 12.0f / (float)Math.Sqrt(len); |
2457 | return Vector3.Zero; | 3046 | newAngVel *= len; |
2458 | |||
2459 | return _torque; | ||
2460 | } | 3047 | } |
2461 | 3048 | ||
2462 | set | 3049 | if (!m_isSelected) |
2463 | { | 3050 | { |
2464 | if (value.IsFinite()) | 3051 | if (Body != IntPtr.Zero) |
2465 | { | ||
2466 | m_taintTorque = value; | ||
2467 | _parent_scene.AddPhysicsActorTaint(this); | ||
2468 | } | ||
2469 | else | ||
2470 | { | 3052 | { |
2471 | m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name); | 3053 | if (m_disabled) |
3054 | enableBodySoft(); | ||
3055 | else if (!d.BodyIsEnabled(Body)) | ||
3056 | d.BodyEnable(Body); | ||
3057 | |||
3058 | |||
3059 | d.BodySetAngularVel(Body, newAngVel.X, newAngVel.Y, newAngVel.Z); | ||
2472 | } | 3060 | } |
3061 | //resetCollisionAccounting(); | ||
2473 | } | 3062 | } |
3063 | m_rotationalVelocity = newAngVel; | ||
2474 | } | 3064 | } |
2475 | 3065 | ||
2476 | public override float CollisionScore | 3066 | private void changeVolumedetetion(bool newVolDtc) |
2477 | { | 3067 | { |
2478 | get { return m_collisionscore; } | 3068 | m_isVolumeDetect = newVolDtc; |
2479 | set { m_collisionscore = value; } | 3069 | m_fakeisVolumeDetect = newVolDtc; |
3070 | UpdateCollisionCatFlags(); | ||
3071 | ApplyCollisionCatFlags(); | ||
2480 | } | 3072 | } |
2481 | 3073 | ||
2482 | public override bool Kinematic | 3074 | protected void changeBuilding(bool newbuilding) |
2483 | { | 3075 | { |
2484 | get { return false; } | 3076 | // Check if we need to do anything |
2485 | set { } | 3077 | if (newbuilding == m_building) |
2486 | } | 3078 | return; |
2487 | 3079 | ||
2488 | public override Quaternion Orientation | 3080 | if ((bool)newbuilding) |
2489 | { | ||
2490 | get { return _orientation; } | ||
2491 | set | ||
2492 | { | 3081 | { |
2493 | if (QuaternionIsFinite(value)) | 3082 | m_building = true; |
2494 | _orientation = value; | 3083 | if (!childPrim) |
2495 | else | 3084 | DestroyBody(); |
2496 | m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name); | 3085 | } |
3086 | else | ||
3087 | { | ||
3088 | m_building = false; | ||
3089 | CheckDelaySelect(); | ||
3090 | if (!childPrim) | ||
3091 | MakeBody(); | ||
3092 | } | ||
3093 | if (!childPrim && childrenPrim.Count > 0) | ||
3094 | { | ||
3095 | foreach (OdePrim prm in childrenPrim) | ||
3096 | prm.changeBuilding(m_building); // call directly | ||
2497 | } | 3097 | } |
2498 | } | 3098 | } |
2499 | 3099 | ||
2500 | private static bool QuaternionIsFinite(Quaternion q) | 3100 | public void changeSetVehicle(VehicleData vdata) |
2501 | { | ||
2502 | if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) | ||
2503 | return false; | ||
2504 | if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) | ||
2505 | return false; | ||
2506 | if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z)) | ||
2507 | return false; | ||
2508 | if (Single.IsNaN(q.W) || Single.IsInfinity(q.W)) | ||
2509 | return false; | ||
2510 | return true; | ||
2511 | } | ||
2512 | |||
2513 | public override Vector3 Acceleration | ||
2514 | { | 3101 | { |
2515 | get { return _acceleration; } | 3102 | if (m_vehicle == null) |
2516 | set { _acceleration = value; } | 3103 | m_vehicle = new ODEDynamics(this); |
3104 | m_vehicle.DoSetVehicle(vdata); | ||
2517 | } | 3105 | } |
2518 | 3106 | ||
2519 | public override void AddForce(Vector3 force, bool pushforce) | 3107 | private void changeVehicleType(int value) |
2520 | { | 3108 | { |
2521 | if (force.IsFinite()) | 3109 | if (value == (int)Vehicle.TYPE_NONE) |
2522 | { | 3110 | { |
2523 | lock (m_forcelist) | 3111 | if (m_vehicle != null) |
2524 | m_forcelist.Add(force); | 3112 | m_vehicle = null; |
2525 | |||
2526 | m_taintforce = true; | ||
2527 | } | 3113 | } |
2528 | else | 3114 | else |
2529 | { | 3115 | { |
2530 | m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name); | 3116 | if (m_vehicle == null) |
3117 | m_vehicle = new ODEDynamics(this); | ||
3118 | |||
3119 | m_vehicle.ProcessTypeChange((Vehicle)value); | ||
2531 | } | 3120 | } |
2532 | //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); | ||
2533 | } | 3121 | } |
2534 | 3122 | ||
2535 | public override void AddAngularForce(Vector3 force, bool pushforce) | 3123 | private void changeVehicleFloatParam(strVehicleFloatParam fp) |
2536 | { | 3124 | { |
2537 | if (force.IsFinite()) | 3125 | if (m_vehicle == null) |
2538 | { | 3126 | return; |
2539 | m_angularforcelist.Add(force); | 3127 | |
2540 | m_taintaddangularforce = true; | 3128 | m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value); |
2541 | } | ||
2542 | else | ||
2543 | { | ||
2544 | m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name); | ||
2545 | } | ||
2546 | } | 3129 | } |
2547 | 3130 | ||
2548 | public override Vector3 RotationalVelocity | 3131 | private void changeVehicleVectorParam(strVehicleVectorParam vp) |
2549 | { | 3132 | { |
2550 | get | 3133 | if (m_vehicle == null) |
2551 | { | 3134 | return; |
2552 | Vector3 pv = Vector3.Zero; | 3135 | m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value); |
2553 | if (_zeroFlag) | 3136 | } |
2554 | return pv; | ||
2555 | m_lastUpdateSent = false; | ||
2556 | |||
2557 | if (m_rotationalVelocity.ApproxEquals(pv, 0.2f)) | ||
2558 | return pv; | ||
2559 | 3137 | ||
2560 | return m_rotationalVelocity; | 3138 | private void changeVehicleRotationParam(strVehicleQuatParam qp) |
2561 | } | 3139 | { |
2562 | set | 3140 | if (m_vehicle == null) |
2563 | { | 3141 | return; |
2564 | if (value.IsFinite()) | 3142 | m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value); |
2565 | { | ||
2566 | m_rotationalVelocity = value; | ||
2567 | } | ||
2568 | else | ||
2569 | { | ||
2570 | m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name); | ||
2571 | } | ||
2572 | } | ||
2573 | } | 3143 | } |
2574 | 3144 | ||
2575 | public override void CrossingFailure() | 3145 | private void changeVehicleFlags(strVehicleBoolParam bp) |
2576 | { | 3146 | { |
2577 | m_crossingfailures++; | 3147 | if (m_vehicle == null) |
2578 | if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds) | ||
2579 | { | ||
2580 | base.RaiseOutOfBounds(_position); | ||
2581 | return; | 3148 | return; |
2582 | } | 3149 | m_vehicle.ProcessVehicleFlags(bp.param, bp.value); |
2583 | else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds) | ||
2584 | { | ||
2585 | m_log.Warn("[PHYSICS]: Too many crossing failures for: " + Name); | ||
2586 | } | ||
2587 | } | 3150 | } |
2588 | 3151 | ||
2589 | public override float Buoyancy | 3152 | private void changeBuoyancy(float b) |
2590 | { | 3153 | { |
2591 | get { return m_buoyancy; } | 3154 | m_buoyancy = b; |
2592 | set { m_buoyancy = value; } | ||
2593 | } | 3155 | } |
2594 | 3156 | ||
2595 | public override void link(PhysicsActor obj) | 3157 | private void changePIDTarget(Vector3 trg) |
2596 | { | 3158 | { |
2597 | m_taintparent = obj; | 3159 | m_PIDTarget = trg; |
2598 | } | 3160 | } |
2599 | 3161 | ||
2600 | public override void delink() | 3162 | private void changePIDTau(float tau) |
2601 | { | 3163 | { |
2602 | m_taintparent = null; | 3164 | m_PIDTau = tau; |
2603 | } | 3165 | } |
2604 | 3166 | ||
2605 | public override void LockAngularMotion(Vector3 axis) | 3167 | private void changePIDActive(bool val) |
2606 | { | 3168 | { |
2607 | // reverse the zero/non zero values for ODE. | 3169 | m_usePID = val; |
2608 | if (axis.IsFinite()) | 3170 | } |
2609 | { | 3171 | |
2610 | axis.X = (axis.X > 0) ? 1f : 0f; | 3172 | private void changePIDHoverHeight(float val) |
2611 | axis.Y = (axis.Y > 0) ? 1f : 0f; | 3173 | { |
2612 | axis.Z = (axis.Z > 0) ? 1f : 0f; | 3174 | m_PIDHoverHeight = val; |
2613 | m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); | 3175 | if (val == 0) |
2614 | m_taintAngularLock = axis; | 3176 | m_useHoverPID = false; |
2615 | } | 3177 | } |
2616 | else | 3178 | |
2617 | { | 3179 | private void changePIDHoverType(PIDHoverType type) |
2618 | m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name); | 3180 | { |
2619 | } | 3181 | m_PIDHoverType = type; |
2620 | } | 3182 | } |
2621 | 3183 | ||
2622 | internal void UpdatePositionAndVelocity() | 3184 | private void changePIDHoverTau(float tau) |
3185 | { | ||
3186 | m_PIDHoverTau = tau; | ||
3187 | } | ||
3188 | |||
3189 | private void changePIDHoverActive(bool active) | ||
3190 | { | ||
3191 | m_useHoverPID = active; | ||
3192 | } | ||
3193 | |||
3194 | #endregion | ||
3195 | |||
3196 | public void Move() | ||
2623 | { | 3197 | { |
2624 | // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! | 3198 | if (!childPrim && m_isphysical && Body != IntPtr.Zero && |
2625 | if (_parent == null) | 3199 | !m_disabled && !m_isSelected && !m_building && !m_outbounds) |
2626 | { | 3200 | { |
2627 | Vector3 pv = Vector3.Zero; | 3201 | if (!d.BodyIsEnabled(Body)) |
2628 | bool lastZeroFlag = _zeroFlag; | 3202 | { |
2629 | float m_minvelocity = 0; | 3203 | // let vehicles sleep |
2630 | if (Body != (IntPtr)0) // FIXME -> or if it is a joint | 3204 | if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) |
2631 | { | 3205 | return; |
2632 | d.Vector3 vec = d.BodyGetPosition(Body); | 3206 | |
2633 | d.Quaternion ori = d.BodyGetQuaternion(Body); | 3207 | if (++bodydisablecontrol < 20) |
2634 | d.Vector3 vel = d.BodyGetLinearVel(Body); | 3208 | return; |
2635 | d.Vector3 rotvel = d.BodyGetAngularVel(Body); | 3209 | |
2636 | d.Vector3 torque = d.BodyGetTorque(Body); | 3210 | |
2637 | _torque = new Vector3(torque.X, torque.Y, torque.Z); | 3211 | d.BodyEnable(Body); |
2638 | Vector3 l_position = Vector3.Zero; | 3212 | } |
2639 | Quaternion l_orientation = Quaternion.Identity; | 3213 | |
2640 | 3214 | bodydisablecontrol = 0; | |
2641 | // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!) | 3215 | |
2642 | //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } | 3216 | d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator |
2643 | //if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } | 3217 | |
2644 | //if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } | 3218 | if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) |
2645 | //if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } | 3219 | { |
2646 | 3220 | // 'VEHICLES' are dealt with in ODEDynamics.cs | |
2647 | m_lastposition = _position; | 3221 | m_vehicle.Step(); |
2648 | m_lastorientation = _orientation; | 3222 | return; |
2649 | 3223 | } | |
2650 | l_position.X = vec.X; | 3224 | |
2651 | l_position.Y = vec.Y; | 3225 | float fx = 0; |
2652 | l_position.Z = vec.Z; | 3226 | float fy = 0; |
2653 | l_orientation.X = ori.X; | 3227 | float fz = 0; |
2654 | l_orientation.Y = ori.Y; | 3228 | |
2655 | l_orientation.Z = ori.Z; | 3229 | float m_mass = _mass; |
2656 | l_orientation.W = ori.W; | 3230 | |
2657 | 3231 | if (m_usePID && m_PIDTau > 0) | |
2658 | if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || l_position.X < 0f || l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || l_position.Y < 0f) | 3232 | { |
3233 | // for now position error | ||
3234 | _target_velocity = | ||
3235 | new Vector3( | ||
3236 | (m_PIDTarget.X - lpos.X), | ||
3237 | (m_PIDTarget.Y - lpos.Y), | ||
3238 | (m_PIDTarget.Z - lpos.Z) | ||
3239 | ); | ||
3240 | |||
3241 | if (_target_velocity.ApproxEquals(Vector3.Zero, 0.02f)) | ||
3242 | { | ||
3243 | d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); | ||
3244 | d.BodySetLinearVel(Body, 0, 0, 0); | ||
3245 | return; | ||
3246 | } | ||
3247 | else | ||
2659 | { | 3248 | { |
2660 | //base.RaiseOutOfBounds(l_position); | 3249 | _zeroFlag = false; |
2661 | 3250 | ||
2662 | if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds) | 3251 | float tmp = 1 / m_PIDTau; |
3252 | _target_velocity *= tmp; | ||
3253 | |||
3254 | // apply limits | ||
3255 | tmp = _target_velocity.Length(); | ||
3256 | if (tmp > 50.0f) | ||
2663 | { | 3257 | { |
2664 | _position = l_position; | 3258 | tmp = 50 / tmp; |
2665 | //_parent_scene.remActivePrim(this); | 3259 | _target_velocity *= tmp; |
2666 | if (_parent == null) | ||
2667 | base.RequestPhysicsterseUpdate(); | ||
2668 | return; | ||
2669 | } | 3260 | } |
2670 | else | 3261 | else if (tmp < 0.05f) |
2671 | { | 3262 | { |
2672 | if (_parent == null) | 3263 | tmp = 0.05f / tmp; |
2673 | base.RaiseOutOfBounds(l_position); | 3264 | _target_velocity *= tmp; |
2674 | return; | ||
2675 | } | 3265 | } |
3266 | |||
3267 | d.Vector3 vel = d.BodyGetLinearVel(Body); | ||
3268 | fx = (_target_velocity.X - vel.X) * m_invTimeStep; | ||
3269 | fy = (_target_velocity.Y - vel.Y) * m_invTimeStep; | ||
3270 | fz = (_target_velocity.Z - vel.Z) * m_invTimeStep; | ||
3271 | // d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); | ||
2676 | } | 3272 | } |
3273 | } // end if (m_usePID) | ||
3274 | |||
3275 | // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller | ||
3276 | else if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0) | ||
3277 | { | ||
2677 | 3278 | ||
2678 | if (l_position.Z < 0) | 3279 | // Non-Vehicles have a limited set of Hover options. |
3280 | // determine what our target height really is based on HoverType | ||
3281 | |||
3282 | m_groundHeight = _parent_scene.GetTerrainHeightAtXY(lpos.X, lpos.Y); | ||
3283 | |||
3284 | switch (m_PIDHoverType) | ||
3285 | { | ||
3286 | case PIDHoverType.Ground: | ||
3287 | m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; | ||
3288 | break; | ||
3289 | |||
3290 | case PIDHoverType.GroundAndWater: | ||
3291 | m_waterHeight = _parent_scene.GetWaterLevel(); | ||
3292 | if (m_groundHeight > m_waterHeight) | ||
3293 | m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; | ||
3294 | else | ||
3295 | m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; | ||
3296 | break; | ||
3297 | } // end switch (m_PIDHoverType) | ||
3298 | |||
3299 | // don't go underground unless volumedetector | ||
3300 | |||
3301 | if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect) | ||
2679 | { | 3302 | { |
2680 | // This is so prim that get lost underground don't fall forever and suck up | 3303 | d.Vector3 vel = d.BodyGetLinearVel(Body); |
2681 | // | 3304 | |
2682 | // Sim resources and memory. | 3305 | fz = (m_targetHoverHeight - lpos.Z); |
2683 | // Disables the prim's movement physics.... | 3306 | |
2684 | // It's a hack and will generate a console message if it fails. | 3307 | // if error is zero, use position control; otherwise, velocity control |
3308 | if (Math.Abs(fz) < 0.01f) | ||
3309 | { | ||
3310 | d.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight); | ||
3311 | d.BodySetLinearVel(Body, vel.X, vel.Y, 0); | ||
3312 | } | ||
3313 | else | ||
3314 | { | ||
3315 | _zeroFlag = false; | ||
3316 | fz /= m_PIDHoverTau; | ||
3317 | |||
3318 | float tmp = Math.Abs(fz); | ||
3319 | if (tmp > 50) | ||
3320 | fz = 50 * Math.Sign(fz); | ||
3321 | else if (tmp < 0.1) | ||
3322 | fz = 0.1f * Math.Sign(fz); | ||
3323 | |||
3324 | fz = ((fz - vel.Z) * m_invTimeStep); | ||
3325 | } | ||
3326 | } | ||
3327 | } | ||
3328 | else | ||
3329 | { | ||
3330 | float b = (1.0f - m_buoyancy); | ||
3331 | fx = _parent_scene.gravityx * b; | ||
3332 | fy = _parent_scene.gravityy * b; | ||
3333 | fz = _parent_scene.gravityz * b; | ||
3334 | } | ||
3335 | |||
3336 | fx *= m_mass; | ||
3337 | fy *= m_mass; | ||
3338 | fz *= m_mass; | ||
3339 | |||
3340 | // constant force | ||
3341 | fx += m_force.X; | ||
3342 | fy += m_force.Y; | ||
3343 | fz += m_force.Z; | ||
3344 | |||
3345 | fx += m_forceacc.X; | ||
3346 | fy += m_forceacc.Y; | ||
3347 | fz += m_forceacc.Z; | ||
3348 | |||
3349 | m_forceacc = Vector3.Zero; | ||
3350 | |||
3351 | //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); | ||
3352 | if (fx != 0 || fy != 0 || fz != 0) | ||
3353 | { | ||
3354 | d.BodyAddForce(Body, fx, fy, fz); | ||
3355 | //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); | ||
3356 | } | ||
3357 | |||
3358 | Vector3 trq; | ||
3359 | |||
3360 | trq = _torque; | ||
3361 | trq += m_angularForceacc; | ||
3362 | m_angularForceacc = Vector3.Zero; | ||
3363 | if (trq.X != 0 || trq.Y != 0 || trq.Z != 0) | ||
3364 | { | ||
3365 | d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z); | ||
3366 | } | ||
3367 | } | ||
3368 | else | ||
3369 | { // is not physical, or is not a body or is selected | ||
3370 | // _zeroPosition = d.BodyGetPosition(Body); | ||
3371 | return; | ||
3372 | //Console.WriteLine("Nothing " + Name); | ||
3373 | |||
3374 | } | ||
3375 | } | ||
3376 | |||
3377 | public void UpdatePositionAndVelocity() | ||
3378 | { | ||
3379 | if (_parent == null && !m_disabled && !m_building && !m_outbounds && Body != IntPtr.Zero) | ||
3380 | { | ||
3381 | if (d.BodyIsEnabled(Body) || !_zeroFlag) | ||
3382 | { | ||
3383 | bool lastZeroFlag = _zeroFlag; | ||
2685 | 3384 | ||
2686 | //IsPhysical = false; | 3385 | d.Vector3 lpos = d.GeomGetPosition(prim_geom); |
2687 | if (_parent == null) | ||
2688 | base.RaiseOutOfBounds(_position); | ||
2689 | 3386 | ||
3387 | // check outside region | ||
3388 | if (lpos.Z < -100 || lpos.Z > 100000f) | ||
3389 | { | ||
3390 | m_outbounds = true; | ||
3391 | |||
3392 | lpos.Z = Util.Clip(lpos.Z, -100f, 100000f); | ||
2690 | _acceleration.X = 0; | 3393 | _acceleration.X = 0; |
2691 | _acceleration.Y = 0; | 3394 | _acceleration.Y = 0; |
2692 | _acceleration.Z = 0; | 3395 | _acceleration.Z = 0; |
@@ -2698,589 +3401,437 @@ Console.WriteLine(" JointCreateFixed"); | |||
2698 | m_rotationalVelocity.Y = 0; | 3401 | m_rotationalVelocity.Y = 0; |
2699 | m_rotationalVelocity.Z = 0; | 3402 | m_rotationalVelocity.Z = 0; |
2700 | 3403 | ||
2701 | if (_parent == null) | 3404 | d.BodySetLinearVel(Body, 0, 0, 0); // stop it |
2702 | base.RequestPhysicsterseUpdate(); | 3405 | d.BodySetAngularVel(Body, 0, 0, 0); // stop it |
3406 | d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere | ||
3407 | m_lastposition = _position; | ||
3408 | m_lastorientation = _orientation; | ||
3409 | |||
3410 | base.RequestPhysicsterseUpdate(); | ||
2703 | 3411 | ||
2704 | m_throttleUpdates = false; | 3412 | // throttleCounter = 0; |
2705 | throttleCounter = 0; | ||
2706 | _zeroFlag = true; | 3413 | _zeroFlag = true; |
2707 | //outofBounds = true; | 3414 | |
3415 | disableBodySoft(); // disable it and colisions | ||
3416 | base.RaiseOutOfBounds(_position); | ||
3417 | return; | ||
2708 | } | 3418 | } |
2709 | 3419 | ||
2710 | //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)); | 3420 | if (lpos.X < 0f) |
2711 | //Console.WriteLine("Adiff " + Name + " = " + Adiff); | ||
2712 | if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) | ||
2713 | && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) | ||
2714 | && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) | ||
2715 | // && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01)) | ||
2716 | && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large | ||
2717 | { | 3421 | { |
2718 | _zeroFlag = true; | 3422 | _position.X = Util.Clip(lpos.X, -2f, -0.1f); |
2719 | //Console.WriteLine("ZFT 2"); | 3423 | m_outbounds = true; |
2720 | m_throttleUpdates = false; | ||
2721 | } | 3424 | } |
2722 | else | 3425 | else if (lpos.X > _parent_scene.WorldExtents.X) |
2723 | { | 3426 | { |
2724 | //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString()); | 3427 | _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); |
2725 | _zeroFlag = false; | 3428 | m_outbounds = true; |
2726 | m_lastUpdateSent = false; | ||
2727 | //m_throttleUpdates = false; | ||
2728 | } | 3429 | } |
2729 | 3430 | if (lpos.Y < 0f) | |
2730 | if (_zeroFlag) | ||
2731 | { | 3431 | { |
2732 | _velocity.X = 0.0f; | 3432 | _position.Y = Util.Clip(lpos.Y, -2f, -0.1f); |
2733 | _velocity.Y = 0.0f; | 3433 | m_outbounds = true; |
2734 | _velocity.Z = 0.0f; | 3434 | } |
2735 | 3435 | else if (lpos.Y > _parent_scene.WorldExtents.Y) | |
2736 | _acceleration.X = 0; | 3436 | { |
2737 | _acceleration.Y = 0; | 3437 | _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); |
2738 | _acceleration.Z = 0; | 3438 | m_outbounds = true; |
2739 | 3439 | } | |
2740 | //_orientation.w = 0f; | ||
2741 | //_orientation.X = 0f; | ||
2742 | //_orientation.Y = 0f; | ||
2743 | //_orientation.Z = 0f; | ||
2744 | m_rotationalVelocity.X = 0; | ||
2745 | m_rotationalVelocity.Y = 0; | ||
2746 | m_rotationalVelocity.Z = 0; | ||
2747 | if (!m_lastUpdateSent) | ||
2748 | { | ||
2749 | m_throttleUpdates = false; | ||
2750 | throttleCounter = 0; | ||
2751 | m_rotationalVelocity = pv; | ||
2752 | 3440 | ||
2753 | if (_parent == null) | 3441 | if (m_outbounds) |
2754 | { | 3442 | { |
2755 | base.RequestPhysicsterseUpdate(); | 3443 | m_lastposition = _position; |
2756 | } | 3444 | m_lastorientation = _orientation; |
3445 | |||
3446 | d.Vector3 dtmp = d.BodyGetAngularVel(Body); | ||
3447 | m_rotationalVelocity.X = dtmp.X; | ||
3448 | m_rotationalVelocity.Y = dtmp.Y; | ||
3449 | m_rotationalVelocity.Z = dtmp.Z; | ||
3450 | |||
3451 | dtmp = d.BodyGetLinearVel(Body); | ||
3452 | _velocity.X = dtmp.X; | ||
3453 | _velocity.Y = dtmp.Y; | ||
3454 | _velocity.Z = dtmp.Z; | ||
3455 | |||
3456 | d.BodySetLinearVel(Body, 0, 0, 0); // stop it | ||
3457 | d.BodySetAngularVel(Body, 0, 0, 0); | ||
3458 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); | ||
3459 | disableBodySoft(); // stop collisions | ||
3460 | UnSubscribeEvents(); | ||
3461 | |||
3462 | base.RequestPhysicsterseUpdate(); | ||
3463 | return; | ||
3464 | } | ||
2757 | 3465 | ||
2758 | m_lastUpdateSent = true; | 3466 | d.Quaternion ori; |
2759 | } | 3467 | d.GeomCopyQuaternion(prim_geom, out ori); |
3468 | |||
3469 | // decide if moving | ||
3470 | // use positions since this are integrated quantities | ||
3471 | // tolerance values depende a lot on simulation noise... | ||
3472 | // use simple math.abs since we dont need to be exact | ||
3473 | |||
3474 | if ( | ||
3475 | (Math.Abs(_position.X - lpos.X) < 0.001f) | ||
3476 | && (Math.Abs(_position.Y - lpos.Y) < 0.001f) | ||
3477 | && (Math.Abs(_position.Z - lpos.Z) < 0.001f) | ||
3478 | && (Math.Abs(_orientation.X - ori.X) < 0.0001f) | ||
3479 | && (Math.Abs(_orientation.Y - ori.Y) < 0.0001f) | ||
3480 | && (Math.Abs(_orientation.Z - ori.Z) < 0.0001f) // ignore W | ||
3481 | ) | ||
3482 | { | ||
3483 | _zeroFlag = true; | ||
2760 | } | 3484 | } |
2761 | else | 3485 | else |
2762 | { | 3486 | _zeroFlag = false; |
2763 | if (lastZeroFlag != _zeroFlag) | ||
2764 | { | ||
2765 | if (_parent == null) | ||
2766 | { | ||
2767 | base.RequestPhysicsterseUpdate(); | ||
2768 | } | ||
2769 | } | ||
2770 | |||
2771 | m_lastVelocity = _velocity; | ||
2772 | 3487 | ||
2773 | _position = l_position; | 3488 | // update velocities and aceleration |
3489 | if (!(_zeroFlag && lastZeroFlag)) | ||
3490 | { | ||
3491 | d.Vector3 vel = d.BodyGetLinearVel(Body); | ||
2774 | 3492 | ||
2775 | _velocity.X = vel.X; | 3493 | _acceleration = _velocity; |
2776 | _velocity.Y = vel.Y; | ||
2777 | _velocity.Z = vel.Z; | ||
2778 | 3494 | ||
2779 | _acceleration = ((_velocity - m_lastVelocity) / 0.1f); | 3495 | if ((Math.Abs(vel.X) < 0.001f) && |
2780 | _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f); | 3496 | (Math.Abs(vel.Y) < 0.001f) && |
2781 | //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString()); | 3497 | (Math.Abs(vel.Z) < 0.001f)) |
2782 | |||
2783 | // Note here that linearvelocity is affecting angular velocity... so I'm guessing this is a vehicle specific thing... | ||
2784 | // it does make sense to do this for tiny little instabilities with physical prim, however 0.5m/frame is fairly large. | ||
2785 | // reducing this to 0.02m/frame seems to help the angular rubberbanding quite a bit, however, to make sure it doesn't affect elevators and vehicles | ||
2786 | // adding these logical exclusion situations to maintain this where I think it was intended to be. | ||
2787 | if (m_throttleUpdates || m_usePID || (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) || (Amotor != IntPtr.Zero)) | ||
2788 | { | 3498 | { |
2789 | m_minvelocity = 0.5f; | 3499 | _velocity = Vector3.Zero; |
3500 | float t = -m_invTimeStep; | ||
3501 | _acceleration = _acceleration * t; | ||
2790 | } | 3502 | } |
2791 | else | 3503 | else |
2792 | { | 3504 | { |
2793 | m_minvelocity = 0.02f; | 3505 | _velocity.X = vel.X; |
3506 | _velocity.Y = vel.Y; | ||
3507 | _velocity.Z = vel.Z; | ||
3508 | _acceleration = (_velocity - _acceleration) * m_invTimeStep; | ||
3509 | } | ||
3510 | |||
3511 | if ((Math.Abs(_acceleration.X) < 0.01f) && | ||
3512 | (Math.Abs(_acceleration.Y) < 0.01f) && | ||
3513 | (Math.Abs(_acceleration.Z) < 0.01f)) | ||
3514 | { | ||
3515 | _acceleration = Vector3.Zero; | ||
2794 | } | 3516 | } |
2795 | 3517 | ||
2796 | if (_velocity.ApproxEquals(pv, m_minvelocity)) | 3518 | if ((Math.Abs(_orientation.X - ori.X) < 0.0001) && |
3519 | (Math.Abs(_orientation.Y - ori.Y) < 0.0001) && | ||
3520 | (Math.Abs(_orientation.Z - ori.Z) < 0.0001) | ||
3521 | ) | ||
2797 | { | 3522 | { |
2798 | m_rotationalVelocity = pv; | 3523 | m_rotationalVelocity = Vector3.Zero; |
2799 | } | 3524 | } |
2800 | else | 3525 | else |
2801 | { | 3526 | { |
2802 | m_rotationalVelocity = new Vector3(rotvel.X, rotvel.Y, rotvel.Z); | 3527 | vel = d.BodyGetAngularVel(Body); |
3528 | m_rotationalVelocity.X = vel.X; | ||
3529 | m_rotationalVelocity.Y = vel.Y; | ||
3530 | m_rotationalVelocity.Z = vel.Z; | ||
2803 | } | 3531 | } |
3532 | } | ||
2804 | 3533 | ||
2805 | //m_log.Debug("ODE: " + m_rotationalVelocity.ToString()); | 3534 | if (_zeroFlag) |
2806 | _orientation.X = ori.X; | 3535 | { |
2807 | _orientation.Y = ori.Y; | 3536 | if (lastZeroFlag) |
2808 | _orientation.Z = ori.Z; | ||
2809 | _orientation.W = ori.W; | ||
2810 | m_lastUpdateSent = false; | ||
2811 | if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) | ||
2812 | { | 3537 | { |
2813 | if (_parent == null) | 3538 | _velocity = Vector3.Zero; |
2814 | { | 3539 | _acceleration = Vector3.Zero; |
2815 | base.RequestPhysicsterseUpdate(); | 3540 | m_rotationalVelocity = Vector3.Zero; |
2816 | } | ||
2817 | } | 3541 | } |
2818 | else | 3542 | |
3543 | if (!m_lastUpdateSent) | ||
2819 | { | 3544 | { |
2820 | throttleCounter++; | 3545 | base.RequestPhysicsterseUpdate(); |
3546 | if (lastZeroFlag) | ||
3547 | m_lastUpdateSent = true; | ||
2821 | } | 3548 | } |
3549 | return; | ||
2822 | } | 3550 | } |
2823 | m_lastposition = l_position; | ||
2824 | } | ||
2825 | else | ||
2826 | { | ||
2827 | // Not a body.. so Make sure the client isn't interpolating | ||
2828 | _velocity.X = 0; | ||
2829 | _velocity.Y = 0; | ||
2830 | _velocity.Z = 0; | ||
2831 | 3551 | ||
2832 | _acceleration.X = 0; | 3552 | _position.X = lpos.X; |
2833 | _acceleration.Y = 0; | 3553 | _position.Y = lpos.Y; |
2834 | _acceleration.Z = 0; | 3554 | _position.Z = lpos.Z; |
2835 | 3555 | ||
2836 | m_rotationalVelocity.X = 0; | 3556 | _orientation.X = ori.X; |
2837 | m_rotationalVelocity.Y = 0; | 3557 | _orientation.Y = ori.Y; |
2838 | m_rotationalVelocity.Z = 0; | 3558 | _orientation.Z = ori.Z; |
2839 | _zeroFlag = true; | 3559 | _orientation.W = ori.W; |
3560 | base.RequestPhysicsterseUpdate(); | ||
3561 | m_lastUpdateSent = false; | ||
2840 | } | 3562 | } |
2841 | } | 3563 | } |
2842 | } | 3564 | } |
2843 | 3565 | ||
2844 | public override bool FloatOnWater | 3566 | internal static bool QuaternionIsFinite(Quaternion q) |
2845 | { | 3567 | { |
2846 | set { | 3568 | if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) |
2847 | m_taintCollidesWater = value; | 3569 | return false; |
2848 | _parent_scene.AddPhysicsActorTaint(this); | 3570 | if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) |
2849 | } | 3571 | return false; |
3572 | if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z)) | ||
3573 | return false; | ||
3574 | if (Single.IsNaN(q.W) || Single.IsInfinity(q.W)) | ||
3575 | return false; | ||
3576 | return true; | ||
2850 | } | 3577 | } |
2851 | 3578 | ||
2852 | public override void SetMomentum(Vector3 momentum) | 3579 | internal static void DMassSubPartFromObj(ref d.Mass part, ref d.Mass theobj) |
2853 | { | 3580 | { |
2854 | } | 3581 | // assumes object center of mass is zero |
2855 | 3582 | float smass = part.mass; | |
2856 | public override Vector3 PIDTarget | 3583 | theobj.mass -= smass; |
2857 | { | ||
2858 | set | ||
2859 | { | ||
2860 | if (value.IsFinite()) | ||
2861 | { | ||
2862 | m_PIDTarget = value; | ||
2863 | } | ||
2864 | else | ||
2865 | m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); | ||
2866 | } | ||
2867 | } | ||
2868 | public override bool PIDActive { set { m_usePID = value; } } | ||
2869 | public override float PIDTau { set { m_PIDTau = value; } } | ||
2870 | |||
2871 | public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } | ||
2872 | public override bool PIDHoverActive { set { m_useHoverPID = value; } } | ||
2873 | public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } | ||
2874 | public override float PIDHoverTau { set { m_PIDHoverTau = value; } } | ||
2875 | |||
2876 | public override Quaternion APIDTarget{ set { return; } } | ||
2877 | 3584 | ||
2878 | public override bool APIDActive{ set { return; } } | 3585 | smass *= 1.0f / (theobj.mass); ; |
2879 | 3586 | ||
2880 | public override float APIDStrength{ set { return; } } | 3587 | theobj.c.X -= part.c.X * smass; |
3588 | theobj.c.Y -= part.c.Y * smass; | ||
3589 | theobj.c.Z -= part.c.Z * smass; | ||
2881 | 3590 | ||
2882 | public override float APIDDamping{ set { return; } } | 3591 | theobj.I.M00 -= part.I.M00; |
3592 | theobj.I.M01 -= part.I.M01; | ||
3593 | theobj.I.M02 -= part.I.M02; | ||
3594 | theobj.I.M10 -= part.I.M10; | ||
3595 | theobj.I.M11 -= part.I.M11; | ||
3596 | theobj.I.M12 -= part.I.M12; | ||
3597 | theobj.I.M20 -= part.I.M20; | ||
3598 | theobj.I.M21 -= part.I.M21; | ||
3599 | theobj.I.M22 -= part.I.M22; | ||
3600 | } | ||
2883 | 3601 | ||
2884 | private void createAMotor(Vector3 axis) | 3602 | private void donullchange() |
2885 | { | 3603 | { |
2886 | if (Body == IntPtr.Zero) | 3604 | } |
2887 | return; | ||
2888 | 3605 | ||
2889 | if (Amotor != IntPtr.Zero) | 3606 | public bool DoAChange(changes what, object arg) |
3607 | { | ||
3608 | if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.AddPhysRep && what != changes.Remove) | ||
2890 | { | 3609 | { |
2891 | d.JointDestroy(Amotor); | 3610 | return false; |
2892 | Amotor = IntPtr.Zero; | ||
2893 | } | 3611 | } |
2894 | 3612 | ||
2895 | float axisnum = 3; | 3613 | // nasty switch |
3614 | switch (what) | ||
3615 | { | ||
3616 | case changes.Add: | ||
3617 | changeadd(); | ||
3618 | break; | ||
2896 | 3619 | ||
2897 | axisnum = (axisnum - (axis.X + axis.Y + axis.Z)); | 3620 | case changes.AddPhysRep: |
3621 | changeAddPhysRep((ODEPhysRepData)arg); | ||
3622 | break; | ||
2898 | 3623 | ||
2899 | // PhysicsVector totalSize = new PhysicsVector(_size.X, _size.Y, _size.Z); | 3624 | case changes.Remove: |
3625 | //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff... | ||
3626 | //When we return true, it destroys all of the prims in the linkset anyway | ||
3627 | if (_parent != null) | ||
3628 | { | ||
3629 | OdePrim parent = (OdePrim)_parent; | ||
3630 | parent.ChildRemove(this, false); | ||
3631 | } | ||
3632 | else | ||
3633 | ChildRemove(this, false); | ||
2900 | 3634 | ||
2901 | 3635 | m_vehicle = null; | |
2902 | // Inverse Inertia Matrix, set the X, Y, and/r Z inertia to 0 then invert it again. | 3636 | RemoveGeom(); |
2903 | d.Mass objMass; | 3637 | m_targetSpace = IntPtr.Zero; |
2904 | d.MassSetZero(out objMass); | 3638 | UnSubscribeEvents(); |
2905 | DMassCopy(ref pMass, ref objMass); | 3639 | return true; |
2906 | 3640 | ||
2907 | //m_log.DebugFormat("1-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22); | 3641 | case changes.Link: |
3642 | OdePrim tmp = (OdePrim)arg; | ||
3643 | changeLink(tmp); | ||
3644 | break; | ||
2908 | 3645 | ||
2909 | Matrix4 dMassMat = FromDMass(objMass); | 3646 | case changes.DeLink: |
3647 | changeLink(null); | ||
3648 | break; | ||
2910 | 3649 | ||
2911 | Matrix4 mathmat = Inverse(dMassMat); | 3650 | case changes.Position: |
3651 | changePosition((Vector3)arg); | ||
3652 | break; | ||
2912 | 3653 | ||
2913 | /* | 3654 | case changes.Orientation: |
2914 | //m_log.DebugFormat("2-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", mathmat[0, 0], mathmat[0, 1], mathmat[0, 2], mathmat[1, 0], mathmat[1, 1], mathmat[1, 2], mathmat[2, 0], mathmat[2, 1], mathmat[2, 2]); | 3655 | changeOrientation((Quaternion)arg); |
3656 | break; | ||
2915 | 3657 | ||
2916 | mathmat = Inverse(mathmat); | 3658 | case changes.PosOffset: |
3659 | donullchange(); | ||
3660 | break; | ||
2917 | 3661 | ||
3662 | case changes.OriOffset: | ||
3663 | donullchange(); | ||
3664 | break; | ||
2918 | 3665 | ||
2919 | objMass = FromMatrix4(mathmat, ref objMass); | 3666 | case changes.Velocity: |
2920 | //m_log.DebugFormat("3-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22); | 3667 | changevelocity((Vector3)arg); |
3668 | break; | ||
2921 | 3669 | ||
2922 | mathmat = Inverse(mathmat); | 3670 | // case changes.Acceleration: |
2923 | */ | 3671 | // changeacceleration((Vector3)arg); |
2924 | if (axis.X == 0) | 3672 | // break; |
2925 | { | ||
2926 | mathmat.M33 = 50.0000001f; | ||
2927 | //objMass.I.M22 = 0; | ||
2928 | } | ||
2929 | if (axis.Y == 0) | ||
2930 | { | ||
2931 | mathmat.M22 = 50.0000001f; | ||
2932 | //objMass.I.M11 = 0; | ||
2933 | } | ||
2934 | if (axis.Z == 0) | ||
2935 | { | ||
2936 | mathmat.M11 = 50.0000001f; | ||
2937 | //objMass.I.M00 = 0; | ||
2938 | } | ||
2939 | |||
2940 | |||
2941 | 3673 | ||
2942 | mathmat = Inverse(mathmat); | 3674 | case changes.AngVelocity: |
2943 | objMass = FromMatrix4(mathmat, ref objMass); | 3675 | changeangvelocity((Vector3)arg); |
2944 | //m_log.DebugFormat("4-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22); | 3676 | break; |
2945 | |||
2946 | //return; | ||
2947 | if (d.MassCheck(ref objMass)) | ||
2948 | { | ||
2949 | d.BodySetMass(Body, ref objMass); | ||
2950 | } | ||
2951 | else | ||
2952 | { | ||
2953 | //m_log.Debug("[PHYSICS]: Mass invalid, ignoring"); | ||
2954 | } | ||
2955 | 3677 | ||
2956 | if (axisnum <= 0) | 3678 | case changes.Force: |
2957 | return; | 3679 | changeForce((Vector3)arg); |
2958 | // int dAMotorEuler = 1; | 3680 | break; |
2959 | 3681 | ||
2960 | Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); | 3682 | case changes.Torque: |
2961 | d.JointAttach(Amotor, Body, IntPtr.Zero); | 3683 | changeSetTorque((Vector3)arg); |
2962 | d.JointSetAMotorMode(Amotor, 0); | 3684 | break; |
2963 | 3685 | ||
2964 | d.JointSetAMotorNumAxes(Amotor,(int)axisnum); | 3686 | case changes.AddForce: |
2965 | int i = 0; | 3687 | changeAddForce((Vector3)arg); |
3688 | break; | ||
2966 | 3689 | ||
2967 | if (axis.X == 0) | 3690 | case changes.AddAngForce: |
2968 | { | 3691 | changeAddAngularImpulse((Vector3)arg); |
2969 | d.JointSetAMotorAxis(Amotor, i, 0, 1, 0, 0); | 3692 | break; |
2970 | i++; | ||
2971 | } | ||
2972 | 3693 | ||
2973 | if (axis.Y == 0) | 3694 | case changes.AngLock: |
2974 | { | 3695 | changeAngularLock((Vector3)arg); |
2975 | d.JointSetAMotorAxis(Amotor, i, 0, 0, 1, 0); | 3696 | break; |
2976 | i++; | ||
2977 | } | ||
2978 | 3697 | ||
2979 | if (axis.Z == 0) | 3698 | case changes.Size: |
2980 | { | 3699 | changeSize((Vector3)arg); |
2981 | d.JointSetAMotorAxis(Amotor, i, 0, 0, 0, 1); | 3700 | break; |
2982 | i++; | ||
2983 | } | ||
2984 | 3701 | ||
2985 | for (int j = 0; j < (int)axisnum; j++) | 3702 | case changes.Shape: |
2986 | { | 3703 | changeShape((PrimitiveBaseShape)arg); |
2987 | //d.JointSetAMotorAngle(Amotor, j, 0); | 3704 | break; |
2988 | } | ||
2989 | 3705 | ||
2990 | //d.JointSetAMotorAngle(Amotor, 1, 0); | 3706 | case changes.PhysRepData: |
2991 | //d.JointSetAMotorAngle(Amotor, 2, 0); | 3707 | changePhysRepData((ODEPhysRepData) arg); |
3708 | break; | ||
2992 | 3709 | ||
2993 | // These lowstops and high stops are effectively (no wiggle room) | 3710 | case changes.CollidesWater: |
2994 | d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0f); | 3711 | changeFloatOnWater((bool)arg); |
2995 | d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f); | 3712 | break; |
2996 | d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0f); | ||
2997 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f); | ||
2998 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); | ||
2999 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f); | ||
3000 | //d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 9000f); | ||
3001 | d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f); | ||
3002 | d.JointSetAMotorParam(Amotor, (int)dParam.FMax, Mass * 50f);// | ||
3003 | } | ||
3004 | 3713 | ||
3005 | private Matrix4 FromDMass(d.Mass pMass) | 3714 | case changes.VolumeDtc: |
3006 | { | 3715 | changeVolumedetetion((bool)arg); |
3007 | Matrix4 obj; | 3716 | break; |
3008 | obj.M11 = pMass.I.M00; | ||
3009 | obj.M12 = pMass.I.M01; | ||
3010 | obj.M13 = pMass.I.M02; | ||
3011 | obj.M14 = 0; | ||
3012 | obj.M21 = pMass.I.M10; | ||
3013 | obj.M22 = pMass.I.M11; | ||
3014 | obj.M23 = pMass.I.M12; | ||
3015 | obj.M24 = 0; | ||
3016 | obj.M31 = pMass.I.M20; | ||
3017 | obj.M32 = pMass.I.M21; | ||
3018 | obj.M33 = pMass.I.M22; | ||
3019 | obj.M34 = 0; | ||
3020 | obj.M41 = 0; | ||
3021 | obj.M42 = 0; | ||
3022 | obj.M43 = 0; | ||
3023 | obj.M44 = 1; | ||
3024 | return obj; | ||
3025 | } | ||
3026 | 3717 | ||
3027 | private d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj) | 3718 | case changes.Phantom: |
3028 | { | 3719 | changePhantomStatus((bool)arg); |
3029 | obj.I.M00 = pMat[0, 0]; | 3720 | break; |
3030 | obj.I.M01 = pMat[0, 1]; | ||
3031 | obj.I.M02 = pMat[0, 2]; | ||
3032 | obj.I.M10 = pMat[1, 0]; | ||
3033 | obj.I.M11 = pMat[1, 1]; | ||
3034 | obj.I.M12 = pMat[1, 2]; | ||
3035 | obj.I.M20 = pMat[2, 0]; | ||
3036 | obj.I.M21 = pMat[2, 1]; | ||
3037 | obj.I.M22 = pMat[2, 2]; | ||
3038 | return obj; | ||
3039 | } | ||
3040 | 3721 | ||
3041 | public override void SubscribeEvents(int ms) | 3722 | case changes.Physical: |
3042 | { | 3723 | changePhysicsStatus((bool)arg); |
3043 | m_eventsubscription = ms; | 3724 | break; |
3044 | _parent_scene.AddCollisionEventReporting(this); | ||
3045 | } | ||
3046 | 3725 | ||
3047 | public override void UnSubscribeEvents() | 3726 | case changes.Selected: |
3048 | { | 3727 | changeSelectedStatus((bool)arg); |
3049 | _parent_scene.RemoveCollisionEventReporting(this); | 3728 | break; |
3050 | m_eventsubscription = 0; | ||
3051 | } | ||
3052 | 3729 | ||
3053 | public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) | 3730 | case changes.disabled: |
3054 | { | 3731 | changeDisable((bool)arg); |
3055 | CollisionEventsThisFrame.AddCollider(CollidedWith, contact); | 3732 | break; |
3056 | } | ||
3057 | 3733 | ||
3058 | public void SendCollisions() | 3734 | case changes.building: |
3059 | { | 3735 | changeBuilding((bool)arg); |
3060 | if (m_collisionsOnPreviousFrame || CollisionEventsThisFrame.Count > 0) | 3736 | break; |
3061 | { | ||
3062 | base.SendCollisionUpdate(CollisionEventsThisFrame); | ||
3063 | 3737 | ||
3064 | if (CollisionEventsThisFrame.Count > 0) | 3738 | case changes.VehicleType: |
3065 | { | 3739 | changeVehicleType((int)arg); |
3066 | m_collisionsOnPreviousFrame = true; | 3740 | break; |
3067 | CollisionEventsThisFrame.Clear(); | ||
3068 | } | ||
3069 | else | ||
3070 | { | ||
3071 | m_collisionsOnPreviousFrame = false; | ||
3072 | } | ||
3073 | } | ||
3074 | } | ||
3075 | 3741 | ||
3076 | public override bool SubscribedEvents() | 3742 | case changes.VehicleFlags: |
3077 | { | 3743 | changeVehicleFlags((strVehicleBoolParam) arg); |
3078 | if (m_eventsubscription > 0) | 3744 | break; |
3079 | return true; | ||
3080 | return false; | ||
3081 | } | ||
3082 | 3745 | ||
3083 | public static Matrix4 Inverse(Matrix4 pMat) | 3746 | case changes.VehicleFloatParam: |
3084 | { | 3747 | changeVehicleFloatParam((strVehicleFloatParam) arg); |
3085 | if (determinant3x3(pMat) == 0) | 3748 | break; |
3086 | { | ||
3087 | return Matrix4.Identity; // should probably throw an error. singluar matrix inverse not possible | ||
3088 | } | ||
3089 | 3749 | ||
3090 | return (Adjoint(pMat) / determinant3x3(pMat)); | 3750 | case changes.VehicleVectorParam: |
3091 | } | 3751 | changeVehicleVectorParam((strVehicleVectorParam) arg); |
3752 | break; | ||
3092 | 3753 | ||
3093 | public static Matrix4 Adjoint(Matrix4 pMat) | 3754 | case changes.VehicleRotationParam: |
3094 | { | 3755 | changeVehicleRotationParam((strVehicleQuatParam) arg); |
3095 | Matrix4 adjointMatrix = new Matrix4(); | 3756 | break; |
3096 | for (int i=0; i<4; i++) | ||
3097 | { | ||
3098 | for (int j=0; j<4; j++) | ||
3099 | { | ||
3100 | Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j))))); | ||
3101 | } | ||
3102 | } | ||
3103 | 3757 | ||
3104 | adjointMatrix = Transpose(adjointMatrix); | 3758 | case changes.SetVehicle: |
3105 | return adjointMatrix; | 3759 | changeSetVehicle((VehicleData) arg); |
3106 | } | 3760 | break; |
3107 | 3761 | ||
3108 | public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol) | 3762 | case changes.Buoyancy: |
3109 | { | 3763 | changeBuoyancy((float)arg); |
3110 | Matrix4 minor = new Matrix4(); | 3764 | break; |
3111 | int m = 0, n = 0; | ||
3112 | for (int i = 0; i < 4; i++) | ||
3113 | { | ||
3114 | if (i == iRow) | ||
3115 | continue; | ||
3116 | n = 0; | ||
3117 | for (int j = 0; j < 4; j++) | ||
3118 | { | ||
3119 | if (j == iCol) | ||
3120 | continue; | ||
3121 | Matrix4SetValue(ref minor, m,n, matrix[i, j]); | ||
3122 | n++; | ||
3123 | } | ||
3124 | m++; | ||
3125 | } | ||
3126 | 3765 | ||
3127 | return minor; | 3766 | case changes.PIDTarget: |
3128 | } | 3767 | changePIDTarget((Vector3)arg); |
3768 | break; | ||
3129 | 3769 | ||
3130 | public static Matrix4 Transpose(Matrix4 pMat) | 3770 | case changes.PIDTau: |
3131 | { | 3771 | changePIDTau((float)arg); |
3132 | Matrix4 transposeMatrix = new Matrix4(); | 3772 | break; |
3133 | for (int i = 0; i < 4; i++) | ||
3134 | for (int j = 0; j < 4; j++) | ||
3135 | Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]); | ||
3136 | return transposeMatrix; | ||
3137 | } | ||
3138 | 3773 | ||
3139 | public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val) | 3774 | case changes.PIDActive: |
3140 | { | 3775 | changePIDActive((bool)arg); |
3141 | switch (r) | 3776 | break; |
3142 | { | ||
3143 | case 0: | ||
3144 | switch (c) | ||
3145 | { | ||
3146 | case 0: | ||
3147 | pMat.M11 = val; | ||
3148 | break; | ||
3149 | case 1: | ||
3150 | pMat.M12 = val; | ||
3151 | break; | ||
3152 | case 2: | ||
3153 | pMat.M13 = val; | ||
3154 | break; | ||
3155 | case 3: | ||
3156 | pMat.M14 = val; | ||
3157 | break; | ||
3158 | } | ||
3159 | 3777 | ||
3778 | case changes.PIDHoverHeight: | ||
3779 | changePIDHoverHeight((float)arg); | ||
3160 | break; | 3780 | break; |
3161 | case 1: | ||
3162 | switch (c) | ||
3163 | { | ||
3164 | case 0: | ||
3165 | pMat.M21 = val; | ||
3166 | break; | ||
3167 | case 1: | ||
3168 | pMat.M22 = val; | ||
3169 | break; | ||
3170 | case 2: | ||
3171 | pMat.M23 = val; | ||
3172 | break; | ||
3173 | case 3: | ||
3174 | pMat.M24 = val; | ||
3175 | break; | ||
3176 | } | ||
3177 | 3781 | ||
3782 | case changes.PIDHoverType: | ||
3783 | changePIDHoverType((PIDHoverType)arg); | ||
3178 | break; | 3784 | break; |
3179 | case 2: | ||
3180 | switch (c) | ||
3181 | { | ||
3182 | case 0: | ||
3183 | pMat.M31 = val; | ||
3184 | break; | ||
3185 | case 1: | ||
3186 | pMat.M32 = val; | ||
3187 | break; | ||
3188 | case 2: | ||
3189 | pMat.M33 = val; | ||
3190 | break; | ||
3191 | case 3: | ||
3192 | pMat.M34 = val; | ||
3193 | break; | ||
3194 | } | ||
3195 | 3785 | ||
3786 | case changes.PIDHoverTau: | ||
3787 | changePIDHoverTau((float)arg); | ||
3196 | break; | 3788 | break; |
3197 | case 3: | ||
3198 | switch (c) | ||
3199 | { | ||
3200 | case 0: | ||
3201 | pMat.M41 = val; | ||
3202 | break; | ||
3203 | case 1: | ||
3204 | pMat.M42 = val; | ||
3205 | break; | ||
3206 | case 2: | ||
3207 | pMat.M43 = val; | ||
3208 | break; | ||
3209 | case 3: | ||
3210 | pMat.M44 = val; | ||
3211 | break; | ||
3212 | } | ||
3213 | 3789 | ||
3790 | case changes.PIDHoverActive: | ||
3791 | changePIDHoverActive((bool)arg); | ||
3792 | break; | ||
3793 | |||
3794 | case changes.Null: | ||
3795 | donullchange(); | ||
3796 | break; | ||
3797 | |||
3798 | |||
3799 | |||
3800 | default: | ||
3801 | donullchange(); | ||
3214 | break; | 3802 | break; |
3215 | } | 3803 | } |
3804 | return false; | ||
3216 | } | 3805 | } |
3217 | 3806 | ||
3218 | private static float determinant3x3(Matrix4 pMat) | 3807 | public void AddChange(changes what, object arg) |
3219 | { | 3808 | { |
3220 | float det = 0; | 3809 | _parent_scene.AddChange((PhysicsActor) this, what, arg); |
3221 | float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2]; | ||
3222 | float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0]; | ||
3223 | float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1]; | ||
3224 | float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2]; | ||
3225 | float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0]; | ||
3226 | float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1]; | ||
3227 | |||
3228 | det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6); | ||
3229 | return det; | ||
3230 | } | ||
3231 | |||
3232 | private static void DMassCopy(ref d.Mass src, ref d.Mass dst) | ||
3233 | { | ||
3234 | dst.c.W = src.c.W; | ||
3235 | dst.c.X = src.c.X; | ||
3236 | dst.c.Y = src.c.Y; | ||
3237 | dst.c.Z = src.c.Z; | ||
3238 | dst.mass = src.mass; | ||
3239 | dst.I.M00 = src.I.M00; | ||
3240 | dst.I.M01 = src.I.M01; | ||
3241 | dst.I.M02 = src.I.M02; | ||
3242 | dst.I.M10 = src.I.M10; | ||
3243 | dst.I.M11 = src.I.M11; | ||
3244 | dst.I.M12 = src.I.M12; | ||
3245 | dst.I.M20 = src.I.M20; | ||
3246 | dst.I.M21 = src.I.M21; | ||
3247 | dst.I.M22 = src.I.M22; | ||
3248 | } | 3810 | } |
3249 | 3811 | ||
3250 | public override void SetMaterial(int pMaterial) | 3812 | |
3813 | private struct strVehicleBoolParam | ||
3251 | { | 3814 | { |
3252 | m_material = pMaterial; | 3815 | public int param; |
3816 | public bool value; | ||
3253 | } | 3817 | } |
3254 | 3818 | ||
3255 | private void CheckMeshAsset() | 3819 | private struct strVehicleFloatParam |
3256 | { | 3820 | { |
3257 | if (_pbs.SculptEntry && !m_assetFailed && _pbs.SculptTexture != UUID.Zero) | 3821 | public int param; |
3258 | { | 3822 | public float value; |
3259 | m_assetFailed = true; | ||
3260 | Util.FireAndForget(delegate | ||
3261 | { | ||
3262 | RequestAssetDelegate assetProvider = _parent_scene.RequestAssetMethod; | ||
3263 | if (assetProvider != null) | ||
3264 | assetProvider(_pbs.SculptTexture, MeshAssetReveived); | ||
3265 | }); | ||
3266 | } | ||
3267 | } | 3823 | } |
3268 | 3824 | ||
3269 | void MeshAssetReveived(AssetBase asset) | 3825 | private struct strVehicleQuatParam |
3270 | { | 3826 | { |
3271 | if (asset.Data != null && asset.Data.Length > 0) | 3827 | public int param; |
3272 | { | 3828 | public Quaternion value; |
3273 | if (!_pbs.SculptEntry) | 3829 | } |
3274 | return; | ||
3275 | if (_pbs.SculptTexture.ToString() != asset.ID) | ||
3276 | return; | ||
3277 | 3830 | ||
3278 | _pbs.SculptData = new byte[asset.Data.Length]; | 3831 | private struct strVehicleVectorParam |
3279 | asset.Data.CopyTo(_pbs.SculptData, 0); | 3832 | { |
3280 | m_assetFailed = false; | 3833 | public int param; |
3281 | m_taintshape = true; | 3834 | public Vector3 value; |
3282 | _parent_scene.AddPhysicsActorTaint(this); | 3835 | } |
3283 | } | ||
3284 | } | ||
3285 | } | 3836 | } |
3286 | } \ No newline at end of file | 3837 | } |
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 7a50c4c..03048a4 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | |||
@@ -25,26 +25,21 @@ | |||
25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. | 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
26 | */ | 26 | */ |
27 | 27 | ||
28 | //#define USE_DRAWSTUFF | ||
29 | //#define SPAM | 28 | //#define SPAM |
30 | 29 | ||
31 | using System; | 30 | using System; |
32 | using System.Collections.Generic; | 31 | using System.Collections.Generic; |
33 | using System.Diagnostics; | ||
34 | using System.IO; | ||
35 | using System.Linq; | ||
36 | using System.Reflection; | 32 | using System.Reflection; |
37 | using System.Runtime.InteropServices; | 33 | using System.Runtime.InteropServices; |
38 | using System.Threading; | 34 | using System.Threading; |
35 | using System.IO; | ||
36 | using System.Diagnostics; | ||
39 | using log4net; | 37 | using log4net; |
40 | using Nini.Config; | 38 | using Nini.Config; |
41 | using Ode.NET; | 39 | using OdeAPI; |
42 | using OpenMetaverse; | ||
43 | #if USE_DRAWSTUFF | ||
44 | using Drawstuff.NET; | ||
45 | #endif | ||
46 | using OpenSim.Framework; | 40 | using OpenSim.Framework; |
47 | using OpenSim.Region.Physics.Manager; | 41 | using OpenSim.Region.Physics.Manager; |
42 | using OpenMetaverse; | ||
48 | 43 | ||
49 | namespace OpenSim.Region.Physics.OdePlugin | 44 | namespace OpenSim.Region.Physics.OdePlugin |
50 | { | 45 | { |
@@ -55,29 +50,42 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
55 | End = 2 | 50 | End = 2 |
56 | } | 51 | } |
57 | 52 | ||
58 | // public struct sCollisionData | 53 | public struct sCollisionData |
59 | // { | 54 | { |
60 | // public uint ColliderLocalId; | 55 | public uint ColliderLocalId; |
61 | // public uint CollidedWithLocalId; | 56 | public uint CollidedWithLocalId; |
62 | // public int NumberOfCollisions; | 57 | public int NumberOfCollisions; |
63 | // public int CollisionType; | 58 | public int CollisionType; |
64 | // public int StatusIndicator; | 59 | public int StatusIndicator; |
65 | // public int lastframe; | 60 | public int lastframe; |
66 | // } | 61 | } |
62 | |||
63 | |||
64 | // colision flags of things others can colide with | ||
65 | // rays, sensors, probes removed since can't be colided with | ||
66 | // The top space where things are placed provided further selection | ||
67 | // ie physical are in active space nonphysical in static | ||
68 | // this should be exclusive as possible | ||
67 | 69 | ||
68 | [Flags] | 70 | [Flags] |
69 | public enum CollisionCategories : int | 71 | public enum CollisionCategories : uint |
70 | { | 72 | { |
71 | Disabled = 0, | 73 | Disabled = 0, |
72 | Geom = 0x00000001, | 74 | //by 'things' types |
73 | Body = 0x00000002, | 75 | Space = 0x01, |
74 | Space = 0x00000004, | 76 | Geom = 0x02, // aka prim/part |
75 | Character = 0x00000008, | 77 | Character = 0x04, |
76 | Land = 0x00000010, | 78 | Land = 0x08, |
77 | Water = 0x00000020, | 79 | Water = 0x010, |
78 | Wind = 0x00000040, | 80 | |
79 | Sensor = 0x00000080, | 81 | // by state |
80 | Selected = 0x00000100 | 82 | Phantom = 0x01000, |
83 | VolumeDtc = 0x02000, | ||
84 | Selected = 0x04000, | ||
85 | NoShape = 0x08000, | ||
86 | |||
87 | |||
88 | All = 0xffffffff | ||
81 | } | 89 | } |
82 | 90 | ||
83 | /// <summary> | 91 | /// <summary> |
@@ -98,400 +106,213 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
98 | /// <summary></summary> | 106 | /// <summary></summary> |
99 | Plastic = 5, | 107 | Plastic = 5, |
100 | /// <summary></summary> | 108 | /// <summary></summary> |
101 | Rubber = 6 | 109 | Rubber = 6, |
110 | |||
111 | light = 7 // compatibility with old viewers | ||
112 | } | ||
113 | |||
114 | public enum changes : int | ||
115 | { | ||
116 | Add = 0, // arg null. finishs the prim creation. should be used internally only ( to remove later ?) | ||
117 | Remove, | ||
118 | Link, // arg AuroraODEPrim new parent prim or null to delink. Makes the prim part of a object with prim parent as root | ||
119 | // or removes from a object if arg is null | ||
120 | DeLink, | ||
121 | Position, // arg Vector3 new position in world coords. Changes prim position. Prim must know if it is root or child | ||
122 | Orientation, // arg Quaternion new orientation in world coords. Changes prim position. Prim must know it it is root or child | ||
123 | PosOffset, // not in use | ||
124 | // arg Vector3 new position in local coords. Changes prim position in object | ||
125 | OriOffset, // not in use | ||
126 | // arg Vector3 new position in local coords. Changes prim position in object | ||
127 | Velocity, | ||
128 | AngVelocity, | ||
129 | Acceleration, | ||
130 | Force, | ||
131 | Torque, | ||
132 | Momentum, | ||
133 | |||
134 | AddForce, | ||
135 | AddAngForce, | ||
136 | AngLock, | ||
137 | |||
138 | Buoyancy, | ||
139 | |||
140 | PIDTarget, | ||
141 | PIDTau, | ||
142 | PIDActive, | ||
143 | |||
144 | PIDHoverHeight, | ||
145 | PIDHoverType, | ||
146 | PIDHoverTau, | ||
147 | PIDHoverActive, | ||
148 | |||
149 | Size, | ||
150 | Shape, | ||
151 | PhysRepData, | ||
152 | AddPhysRep, | ||
153 | |||
154 | CollidesWater, | ||
155 | VolumeDtc, | ||
156 | |||
157 | Physical, | ||
158 | Phantom, | ||
159 | Selected, | ||
160 | disabled, | ||
161 | building, | ||
162 | |||
163 | VehicleType, | ||
164 | VehicleFloatParam, | ||
165 | VehicleVectorParam, | ||
166 | VehicleRotationParam, | ||
167 | VehicleFlags, | ||
168 | SetVehicle, | ||
169 | |||
170 | Null //keep this last used do dim the methods array. does nothing but pulsing the prim | ||
102 | } | 171 | } |
103 | 172 | ||
173 | public struct ODEchangeitem | ||
174 | { | ||
175 | public PhysicsActor actor; | ||
176 | public OdeCharacter character; | ||
177 | public changes what; | ||
178 | public Object arg; | ||
179 | } | ||
180 | |||
104 | public class OdeScene : PhysicsScene | 181 | public class OdeScene : PhysicsScene |
105 | { | 182 | { |
106 | private readonly ILog m_log; | 183 | private readonly ILog m_log; |
107 | // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>(); | 184 | // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>(); |
108 | 185 | ||
109 | /// <summary> | 186 | public bool OdeUbitLib = false; |
110 | /// Provide a sync object so that only one thread calls d.Collide() at a time across all OdeScene instances. | 187 | // private int threadid = 0; |
111 | /// </summary> | 188 | private Random fluidRandomizer = new Random(Environment.TickCount); |
112 | /// <remarks> | ||
113 | /// With ODE as of r1755 (though also tested on r1860), only one thread can call d.Collide() at a | ||
114 | /// time, even where physics objects are in entirely different ODE worlds. This is because generating contacts | ||
115 | /// uses a static cache at the ODE level. | ||
116 | /// | ||
117 | /// Without locking, simulators running multiple regions will eventually crash with a native stack trace similar | ||
118 | /// to | ||
119 | /// | ||
120 | /// mono() [0x489171] | ||
121 | /// mono() [0x4d154f] | ||
122 | /// /lib/x86_64-linux-gnu/libpthread.so.0(+0xfc60) [0x7f6ded592c60] | ||
123 | /// .../opensim/bin/libode-x86_64.so(_ZN6Opcode11OBBCollider8_CollideEPKNS_14AABBNoLeafNodeE+0xd7a) [0x7f6dd822628a] | ||
124 | /// | ||
125 | /// ODE provides an experimental option to cache in thread local storage but compiling ODE with this option | ||
126 | /// causes OpenSimulator to immediately crash with a native stack trace similar to | ||
127 | /// | ||
128 | /// mono() [0x489171] | ||
129 | /// mono() [0x4d154f] | ||
130 | /// /lib/x86_64-linux-gnu/libpthread.so.0(+0xfc60) [0x7f03c9849c60] | ||
131 | /// .../opensim/bin/libode-x86_64.so(_Z12dCollideCCTLP6dxGeomS0_iP12dContactGeomi+0x92) [0x7f03b44bcf82] | ||
132 | /// </remarks> | ||
133 | internal static Object UniversalColliderSyncObject = new Object(); | ||
134 | |||
135 | /// <summary> | ||
136 | /// Is stats collecting enabled for this ODE scene? | ||
137 | /// </summary> | ||
138 | public bool CollectStats { get; set; } | ||
139 | |||
140 | /// <summary> | ||
141 | /// Statistics for this scene. | ||
142 | /// </summary> | ||
143 | private Dictionary<string, float> m_stats = new Dictionary<string, float>(); | ||
144 | |||
145 | /// <summary> | ||
146 | /// Stat name for total number of avatars in this ODE scene. | ||
147 | /// </summary> | ||
148 | public const string ODETotalAvatarsStatName = "ODETotalAvatars"; | ||
149 | |||
150 | /// <summary> | ||
151 | /// Stat name for total number of prims in this ODE scene. | ||
152 | /// </summary> | ||
153 | public const string ODETotalPrimsStatName = "ODETotalPrims"; | ||
154 | |||
155 | /// <summary> | ||
156 | /// Stat name for total number of prims with active physics in this ODE scene. | ||
157 | /// </summary> | ||
158 | public const string ODEActivePrimsStatName = "ODEActivePrims"; | ||
159 | |||
160 | /// <summary> | ||
161 | /// Stat name for the total time spent in ODE frame processing. | ||
162 | /// </summary> | ||
163 | /// <remarks> | ||
164 | /// A sanity check for the main scene loop physics time. | ||
165 | /// </remarks> | ||
166 | public const string ODETotalFrameMsStatName = "ODETotalFrameMS"; | ||
167 | |||
168 | /// <summary> | ||
169 | /// Stat name for time spent processing avatar taints per frame | ||
170 | /// </summary> | ||
171 | public const string ODEAvatarTaintMsStatName = "ODEAvatarTaintFrameMS"; | ||
172 | |||
173 | /// <summary> | ||
174 | /// Stat name for time spent processing prim taints per frame | ||
175 | /// </summary> | ||
176 | public const string ODEPrimTaintMsStatName = "ODEPrimTaintFrameMS"; | ||
177 | |||
178 | /// <summary> | ||
179 | /// Stat name for time spent calculating avatar forces per frame. | ||
180 | /// </summary> | ||
181 | public const string ODEAvatarForcesFrameMsStatName = "ODEAvatarForcesFrameMS"; | ||
182 | |||
183 | /// <summary> | ||
184 | /// Stat name for time spent calculating prim forces per frame | ||
185 | /// </summary> | ||
186 | public const string ODEPrimForcesFrameMsStatName = "ODEPrimForcesFrameMS"; | ||
187 | |||
188 | /// <summary> | ||
189 | /// Stat name for time spent fulfilling raycasting requests per frame | ||
190 | /// </summary> | ||
191 | public const string ODERaycastingFrameMsStatName = "ODERaycastingFrameMS"; | ||
192 | |||
193 | /// <summary> | ||
194 | /// Stat name for time spent in native code that actually steps through the simulation. | ||
195 | /// </summary> | ||
196 | public const string ODENativeStepFrameMsStatName = "ODENativeStepFrameMS"; | ||
197 | |||
198 | /// <summary> | ||
199 | /// Stat name for the number of milliseconds that ODE spends in native space collision code. | ||
200 | /// </summary> | ||
201 | public const string ODENativeSpaceCollisionFrameMsStatName = "ODENativeSpaceCollisionFrameMS"; | ||
202 | |||
203 | /// <summary> | ||
204 | /// Stat name for milliseconds that ODE spends in native geom collision code. | ||
205 | /// </summary> | ||
206 | public const string ODENativeGeomCollisionFrameMsStatName = "ODENativeGeomCollisionFrameMS"; | ||
207 | |||
208 | /// <summary> | ||
209 | /// Time spent in collision processing that is not spent in native space or geom collision code. | ||
210 | /// </summary> | ||
211 | public const string ODEOtherCollisionFrameMsStatName = "ODEOtherCollisionFrameMS"; | ||
212 | |||
213 | /// <summary> | ||
214 | /// Stat name for time spent notifying listeners of collisions | ||
215 | /// </summary> | ||
216 | public const string ODECollisionNotificationFrameMsStatName = "ODECollisionNotificationFrameMS"; | ||
217 | |||
218 | /// <summary> | ||
219 | /// Stat name for milliseconds spent updating avatar position and velocity | ||
220 | /// </summary> | ||
221 | public const string ODEAvatarUpdateFrameMsStatName = "ODEAvatarUpdateFrameMS"; | ||
222 | |||
223 | /// <summary> | ||
224 | /// Stat name for the milliseconds spent updating prim position and velocity | ||
225 | /// </summary> | ||
226 | public const string ODEPrimUpdateFrameMsStatName = "ODEPrimUpdateFrameMS"; | ||
227 | |||
228 | /// <summary> | ||
229 | /// Stat name for avatar collisions with another entity. | ||
230 | /// </summary> | ||
231 | public const string ODEAvatarContactsStatsName = "ODEAvatarContacts"; | ||
232 | |||
233 | /// <summary> | ||
234 | /// Stat name for prim collisions with another entity. | ||
235 | /// </summary> | ||
236 | public const string ODEPrimContactsStatName = "ODEPrimContacts"; | ||
237 | |||
238 | /// <summary> | ||
239 | /// Used to hold tick numbers for stat collection purposes. | ||
240 | /// </summary> | ||
241 | private int m_nativeCollisionStartTick; | ||
242 | |||
243 | /// <summary> | ||
244 | /// A messy way to tell if we need to avoid adding a collision time because this was already done in the callback. | ||
245 | /// </summary> | ||
246 | private bool m_inCollisionTiming; | ||
247 | |||
248 | /// <summary> | ||
249 | /// A temporary holder for the number of avatar collisions in a frame, so we can work out how many object | ||
250 | /// collisions occured using the _perloopcontact if stats collection is enabled. | ||
251 | /// </summary> | ||
252 | private int m_tempAvatarCollisionsThisFrame; | ||
253 | 189 | ||
254 | /// <summary> | 190 | const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce; |
255 | /// Used in calculating physics frame time dilation | 191 | const float MaxERP = 0.8f; |
256 | /// </summary> | 192 | const float minERP = 0.1f; |
257 | private int tickCountFrameRun; | 193 | const float comumContactCFM = 0.0001f; |
194 | |||
195 | float frictionMovementMult = 0.8f; | ||
258 | 196 | ||
259 | /// <summary> | 197 | float TerrainBounce = 0.1f; |
260 | /// Used in calculating physics frame time dilation | 198 | float TerrainFriction = 0.3f; |
261 | /// </summary> | ||
262 | private int latertickcount; | ||
263 | 199 | ||
264 | private Random fluidRandomizer = new Random(Environment.TickCount); | 200 | public float AvatarFriction = 0;// 0.9f * 0.5f; |
265 | 201 | ||
266 | private const uint m_regionWidth = Constants.RegionSize; | 202 | private const uint m_regionWidth = Constants.RegionSize; |
267 | private const uint m_regionHeight = Constants.RegionSize; | 203 | private const uint m_regionHeight = Constants.RegionSize; |
268 | 204 | ||
269 | private float ODE_STEPSIZE = 0.0178f; | 205 | public float ODE_STEPSIZE = 0.020f; |
270 | private float metersInSpace = 29.9f; | 206 | public float HalfOdeStep = 0.01f; |
207 | public int odetimestepMS = 20; // rounded | ||
208 | private float metersInSpace = 25.6f; | ||
271 | private float m_timeDilation = 1.0f; | 209 | private float m_timeDilation = 1.0f; |
272 | 210 | ||
211 | private DateTime m_lastframe; | ||
212 | private DateTime m_lastMeshExpire; | ||
213 | |||
273 | public float gravityx = 0f; | 214 | public float gravityx = 0f; |
274 | public float gravityy = 0f; | 215 | public float gravityy = 0f; |
275 | public float gravityz = -9.8f; | 216 | public float gravityz = -9.8f; |
276 | 217 | ||
277 | public float AvatarTerminalVelocity { get; set; } | ||
278 | |||
279 | private float contactsurfacelayer = 0.001f; | ||
280 | |||
281 | private int worldHashspaceLow = -4; | ||
282 | private int worldHashspaceHigh = 128; | ||
283 | |||
284 | private int smallHashspaceLow = -4; | ||
285 | private int smallHashspaceHigh = 66; | ||
286 | |||
287 | private float waterlevel = 0f; | 218 | private float waterlevel = 0f; |
288 | private int framecount = 0; | 219 | private int framecount = 0; |
289 | //private int m_returncollisions = 10; | ||
290 | |||
291 | private readonly IntPtr contactgroup; | ||
292 | |||
293 | internal IntPtr WaterGeom; | ||
294 | 220 | ||
295 | private float nmTerrainContactFriction = 255.0f; | 221 | private int m_meshExpireCntr; |
296 | private float nmTerrainContactBounce = 0.1f; | ||
297 | private float nmTerrainContactERP = 0.1025f; | ||
298 | 222 | ||
299 | private float mTerrainContactFriction = 75f; | 223 | // private IntPtr WaterGeom = IntPtr.Zero; |
300 | private float mTerrainContactBounce = 0.1f; | 224 | // private IntPtr WaterHeightmapData = IntPtr.Zero; |
301 | private float mTerrainContactERP = 0.05025f; | 225 | // private GCHandle WaterMapHandler = new GCHandle(); |
302 | 226 | ||
303 | private float nmAvatarObjectContactFriction = 250f; | 227 | public float avPIDD = 2200f; // make it visible |
304 | private float nmAvatarObjectContactBounce = 0.1f; | 228 | public float avPIDP = 900f; // make it visible |
305 | |||
306 | private float mAvatarObjectContactFriction = 75f; | ||
307 | private float mAvatarObjectContactBounce = 0.1f; | ||
308 | |||
309 | private float avPIDD = 3200f; | ||
310 | private float avPIDP = 1400f; | ||
311 | private float avCapRadius = 0.37f; | 229 | private float avCapRadius = 0.37f; |
312 | private float avStandupTensor = 2000000f; | 230 | private float avDensity = 3f; |
313 | |||
314 | /// <summary> | ||
315 | /// true = old compatibility mode with leaning capsule; false = new corrected mode | ||
316 | /// </summary> | ||
317 | /// <remarks> | ||
318 | /// Even when set to false, the capsule still tilts but this is done in a different way. | ||
319 | /// </remarks> | ||
320 | public bool IsAvCapsuleTilted { get; private set; } | ||
321 | |||
322 | private float avDensity = 80f; | ||
323 | // private float avHeightFudgeFactor = 0.52f; | ||
324 | private float avMovementDivisorWalk = 1.3f; | 231 | private float avMovementDivisorWalk = 1.3f; |
325 | private float avMovementDivisorRun = 0.8f; | 232 | private float avMovementDivisorRun = 0.8f; |
326 | private float minimumGroundFlightOffset = 3f; | 233 | private float minimumGroundFlightOffset = 3f; |
327 | public float maximumMassObject = 10000.01f; | 234 | public float maximumMassObject = 10000.01f; |
328 | 235 | ||
329 | public bool meshSculptedPrim = true; | ||
330 | public bool forceSimplePrimMeshing = false; | ||
331 | |||
332 | public float meshSculptLOD = 32; | ||
333 | public float MeshSculptphysicalLOD = 16; | ||
334 | 236 | ||
335 | public float geomDefaultDensity = 10.000006836f; | 237 | public float geomDefaultDensity = 10.000006836f; |
336 | 238 | ||
337 | public int geomContactPointsStartthrottle = 3; | 239 | public int geomContactPointsStartthrottle = 3; |
338 | public int geomUpdatesPerThrottledUpdate = 15; | 240 | public int geomUpdatesPerThrottledUpdate = 15; |
339 | private const int avatarExpectedContacts = 3; | ||
340 | 241 | ||
341 | public float bodyPIDD = 35f; | 242 | public float bodyPIDD = 35f; |
342 | public float bodyPIDG = 25; | 243 | public float bodyPIDG = 25; |
343 | 244 | ||
344 | public int geomCrossingFailuresBeforeOutofbounds = 5; | 245 | // public int geomCrossingFailuresBeforeOutofbounds = 6; |
345 | |||
346 | public float bodyMotorJointMaxforceTensor = 2; | ||
347 | 246 | ||
348 | public int bodyFramesAutoDisable = 20; | 247 | public int bodyFramesAutoDisable = 5; |
349 | 248 | ||
350 | private float[] _watermap; | ||
351 | private bool m_filterCollisions = true; | ||
352 | 249 | ||
353 | private d.NearCallback nearCallback; | 250 | private d.NearCallback nearCallback; |
354 | public d.TriCallback triCallback; | ||
355 | public d.TriArrayCallback triArrayCallback; | ||
356 | 251 | ||
357 | /// <summary> | 252 | private HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>(); |
358 | /// Avatars in the physics scene. | 253 | private HashSet<OdePrim> _prims = new HashSet<OdePrim>(); |
359 | /// </summary> | 254 | private HashSet<OdePrim> _activeprims = new HashSet<OdePrim>(); |
360 | private readonly HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>(); | 255 | private HashSet<OdePrim> _activegroups = new HashSet<OdePrim>(); |
361 | 256 | ||
362 | /// <summary> | 257 | public OpenSim.Framework.LocklessQueue<ODEchangeitem> ChangesQueue = new OpenSim.Framework.LocklessQueue<ODEchangeitem>(); |
363 | /// Prims in the physics scene. | ||
364 | /// </summary> | ||
365 | private readonly HashSet<OdePrim> _prims = new HashSet<OdePrim>(); | ||
366 | 258 | ||
367 | /// <summary> | 259 | /// <summary> |
368 | /// Prims in the physics scene that are subject to physics, not just collisions. | 260 | /// A list of actors that should receive collision events. |
369 | /// </summary> | 261 | /// </summary> |
370 | private readonly HashSet<OdePrim> _activeprims = new HashSet<OdePrim>(); | 262 | private List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>(); |
371 | 263 | private List<PhysicsActor> _collisionEventPrimRemove = new List<PhysicsActor>(); | |
372 | /// <summary> | 264 | |
373 | /// Prims that the simulator has created/deleted/updated and so need updating in ODE. | 265 | private HashSet<OdeCharacter> _badCharacter = new HashSet<OdeCharacter>(); |
374 | /// </summary> | 266 | // public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>(); |
375 | private readonly HashSet<OdePrim> _taintedPrims = new HashSet<OdePrim>(); | 267 | public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>(); |
376 | |||
377 | /// <summary> | ||
378 | /// Record a character that has taints to be processed. | ||
379 | /// </summary> | ||
380 | private readonly HashSet<OdeCharacter> _taintedActors = new HashSet<OdeCharacter>(); | ||
381 | 268 | ||
382 | /// <summary> | 269 | private float contactsurfacelayer = 0.002f; |
383 | /// Keep record of contacts in the physics loop so that we can remove duplicates. | ||
384 | /// </summary> | ||
385 | private readonly List<d.ContactGeom> _perloopContact = new List<d.ContactGeom>(); | ||
386 | 270 | ||
387 | /// <summary> | 271 | private int contactsPerCollision = 80; |
388 | /// A dictionary of actors that should receive collision events. | 272 | internal IntPtr ContactgeomsArray = IntPtr.Zero; |
389 | /// </summary> | 273 | private IntPtr GlobalContactsArray = IntPtr.Zero; |
390 | private readonly Dictionary<uint, PhysicsActor> m_collisionEventActors = new Dictionary<uint, PhysicsActor>(); | ||
391 | 274 | ||
392 | /// <summary> | 275 | const int maxContactsbeforedeath = 4000; |
393 | /// A dictionary of collision event changes that are waiting to be processed. | 276 | private volatile int m_global_contactcount = 0; |
394 | /// </summary> | ||
395 | private readonly Dictionary<uint, PhysicsActor> m_collisionEventActorsChanges = new Dictionary<uint, PhysicsActor>(); | ||
396 | 277 | ||
397 | /// <summary> | 278 | private IntPtr contactgroup; |
398 | /// Maps a unique geometry id (a memory location) to a physics actor name. | ||
399 | /// </summary> | ||
400 | /// <remarks> | ||
401 | /// Only actors participating in collisions have geometries. This has to be maintained separately from | ||
402 | /// actor_name_map because terrain and water currently don't conceptually have a physics actor of their own | ||
403 | /// apart from the singleton PANull | ||
404 | /// </remarks> | ||
405 | public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>(); | ||
406 | 279 | ||
407 | /// <summary> | 280 | public ContactData[] m_materialContactsData = new ContactData[8]; |
408 | /// Maps a unique geometry id (a memory location) to a physics actor. | ||
409 | /// </summary> | ||
410 | /// <remarks> | ||
411 | /// Only actors participating in collisions have geometries. | ||
412 | /// </remarks> | ||
413 | public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>(); | ||
414 | 281 | ||
415 | /// <summary> | 282 | private Dictionary<Vector3, IntPtr> RegionTerrain = new Dictionary<Vector3, IntPtr>(); |
416 | /// Defects list to remove characters that no longer have finite positions due to some other bug. | 283 | private Dictionary<IntPtr, float[]> TerrainHeightFieldHeights = new Dictionary<IntPtr, float[]>(); |
417 | /// </summary> | 284 | private Dictionary<IntPtr, GCHandle> TerrainHeightFieldHeightsHandlers = new Dictionary<IntPtr, GCHandle>(); |
418 | /// <remarks> | 285 | |
419 | /// Used repeatedly in Simulate() but initialized once here. | 286 | private int m_physicsiterations = 10; |
420 | /// </remarks> | 287 | private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag |
421 | private readonly List<OdeCharacter> defects = new List<OdeCharacter>(); | 288 | // private PhysicsActor PANull = new NullPhysicsActor(); |
289 | private float step_time = 0.0f; | ||
422 | 290 | ||
423 | private bool m_NINJA_physics_joints_enabled = false; | 291 | public IntPtr world; |
424 | //private Dictionary<String, IntPtr> jointpart_name_map = new Dictionary<String,IntPtr>(); | ||
425 | private readonly Dictionary<String, List<PhysicsJoint>> joints_connecting_actor = new Dictionary<String, List<PhysicsJoint>>(); | ||
426 | private d.ContactGeom[] contacts; | ||
427 | 292 | ||
428 | /// <summary> | ||
429 | /// Lock only briefly. accessed by external code (to request new joints) and by OdeScene.Simulate() to move those joints into pending/active | ||
430 | /// </summary> | ||
431 | private readonly List<PhysicsJoint> requestedJointsToBeCreated = new List<PhysicsJoint>(); | ||
432 | 293 | ||
433 | /// <summary> | 294 | // split the spaces acording to contents type |
434 | /// can lock for longer. accessed only by OdeScene. | 295 | // ActiveSpace contains characters and active prims |
435 | /// </summary> | 296 | // StaticSpace contains land and other that is mostly static in enviroment |
436 | private readonly List<PhysicsJoint> pendingJoints = new List<PhysicsJoint>(); | 297 | // this can contain subspaces, like the grid in staticspace |
298 | // as now space only contains this 2 top spaces | ||
437 | 299 | ||
438 | /// <summary> | 300 | public IntPtr TopSpace; // the global space |
439 | /// can lock for longer. accessed only by OdeScene. | 301 | public IntPtr ActiveSpace; // space for active prims |
440 | /// </summary> | 302 | public IntPtr StaticSpace; // space for the static things around |
441 | private readonly List<PhysicsJoint> activeJoints = new List<PhysicsJoint>(); | 303 | public IntPtr GroundSpace; // space for ground |
442 | 304 | ||
443 | /// <summary> | 305 | // some speedup variables |
444 | /// lock only briefly. accessed by external code (to request deletion of joints) and by OdeScene.Simulate() to move those joints out of pending/active | 306 | private int spaceGridMaxX; |
445 | /// </summary> | 307 | private int spaceGridMaxY; |
446 | private readonly List<string> requestedJointsToBeDeleted = new List<string>(); | 308 | private float spacesPerMeter; |
447 | |||
448 | private Object externalJointRequestsLock = new Object(); | ||
449 | private readonly Dictionary<String, PhysicsJoint> SOPName_to_activeJoint = new Dictionary<String, PhysicsJoint>(); | ||
450 | private readonly Dictionary<String, PhysicsJoint> SOPName_to_pendingJoint = new Dictionary<String, PhysicsJoint>(); | ||
451 | private readonly DoubleDictionary<Vector3, IntPtr, IntPtr> RegionTerrain = new DoubleDictionary<Vector3, IntPtr, IntPtr>(); | ||
452 | private readonly Dictionary<IntPtr,float[]> TerrainHeightFieldHeights = new Dictionary<IntPtr, float[]>(); | ||
453 | |||
454 | private d.Contact contact; | ||
455 | private d.Contact TerrainContact; | ||
456 | private d.Contact AvatarMovementprimContact; | ||
457 | private d.Contact AvatarMovementTerrainContact; | ||
458 | private d.Contact WaterContact; | ||
459 | private d.Contact[,] m_materialContacts; | ||
460 | |||
461 | //Ckrinke: Comment out until used. We declare it, initialize it, but do not use it | ||
462 | //Ckrinke private int m_randomizeWater = 200; | ||
463 | private int m_physicsiterations = 10; | ||
464 | private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag | ||
465 | private readonly PhysicsActor PANull = new NullPhysicsActor(); | ||
466 | // private float step_time = 0.0f; | ||
467 | //Ckrinke: Comment out until used. We declare it, initialize it, but do not use it | ||
468 | //Ckrinke private int ms = 0; | ||
469 | public IntPtr world; | ||
470 | //private bool returncollisions = false; | ||
471 | // private uint obj1LocalID = 0; | ||
472 | private uint obj2LocalID = 0; | ||
473 | //private int ctype = 0; | ||
474 | private OdeCharacter cc1; | ||
475 | private OdePrim cp1; | ||
476 | private OdeCharacter cc2; | ||
477 | private OdePrim cp2; | ||
478 | private int p1ExpectedPoints = 0; | ||
479 | private int p2ExpectedPoints = 0; | ||
480 | //private int cStartStop = 0; | ||
481 | //private string cDictKey = ""; | ||
482 | |||
483 | public IntPtr space; | ||
484 | |||
485 | //private IntPtr tmpSpace; | ||
486 | // split static geometry collision handling into spaces of 30 meters | ||
487 | public IntPtr[,] staticPrimspace; | ||
488 | 309 | ||
489 | /// <summary> | 310 | // split static geometry collision into a grid as before |
490 | /// Used to lock the entire physics scene. Locked during the main part of Simulate() | 311 | private IntPtr[,] staticPrimspace; |
491 | /// </summary> | 312 | private IntPtr[] staticPrimspaceOffRegion; |
492 | internal Object OdeLock = new Object(); | ||
493 | 313 | ||
494 | private bool _worldInitialized = false; | 314 | public Object OdeLock; |
315 | public static Object SimulationLock; | ||
495 | 316 | ||
496 | public IMesher mesher; | 317 | public IMesher mesher; |
497 | 318 | ||
@@ -501,461 +322,340 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
501 | public int physics_logging_interval = 0; | 322 | public int physics_logging_interval = 0; |
502 | public bool physics_logging_append_existing_logfile = false; | 323 | public bool physics_logging_append_existing_logfile = false; |
503 | 324 | ||
504 | |||
505 | public d.Vector3 xyz = new d.Vector3(128.1640f, 128.3079f, 25.7600f); | ||
506 | public d.Vector3 hpr = new d.Vector3(125.5000f, -17.0000f, 0.0000f); | ||
507 | |||
508 | // TODO: unused: private uint heightmapWidth = m_regionWidth + 1; | ||
509 | // TODO: unused: private uint heightmapHeight = m_regionHeight + 1; | ||
510 | // TODO: unused: private uint heightmapWidthSamples; | ||
511 | // TODO: unused: private uint heightmapHeightSamples; | ||
512 | |||
513 | private volatile int m_global_contactcount = 0; | ||
514 | |||
515 | private Vector3 m_worldOffset = Vector3.Zero; | 325 | private Vector3 m_worldOffset = Vector3.Zero; |
516 | public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); | 326 | public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); |
517 | private PhysicsScene m_parentScene = null; | 327 | private PhysicsScene m_parentScene = null; |
518 | 328 | ||
519 | private ODERayCastRequestManager m_rayCastManager; | 329 | private ODERayCastRequestManager m_rayCastManager; |
330 | public ODEMeshWorker m_meshWorker; | ||
331 | |||
332 | /* maybe needed if ode uses tls | ||
333 | private void checkThread() | ||
334 | { | ||
520 | 335 | ||
336 | int th = Thread.CurrentThread.ManagedThreadId; | ||
337 | if(th != threadid) | ||
338 | { | ||
339 | threadid = th; | ||
340 | d.AllocateODEDataForThread(~0U); | ||
341 | } | ||
342 | } | ||
343 | */ | ||
521 | /// <summary> | 344 | /// <summary> |
522 | /// Initiailizes the scene | 345 | /// Initiailizes the scene |
523 | /// Sets many properties that ODE requires to be stable | 346 | /// Sets many properties that ODE requires to be stable |
524 | /// These settings need to be tweaked 'exactly' right or weird stuff happens. | 347 | /// These settings need to be tweaked 'exactly' right or weird stuff happens. |
525 | /// </summary> | 348 | /// </summary> |
526 | /// <param value="name">Name of the scene. Useful in debug messages.</param> | 349 | public OdeScene(string sceneIdentifier) |
527 | public OdeScene(string name) | 350 | { |
528 | { | 351 | m_log |
529 | m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + name); | 352 | = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + sceneIdentifier); |
530 | 353 | ||
531 | Name = name; | 354 | // checkThread(); |
355 | Name = sceneIdentifier; | ||
356 | |||
357 | OdeLock = new Object(); | ||
358 | SimulationLock = new Object(); | ||
532 | 359 | ||
533 | nearCallback = near; | 360 | nearCallback = near; |
534 | triCallback = TriCallback; | 361 | |
535 | triArrayCallback = TriArrayCallback; | ||
536 | m_rayCastManager = new ODERayCastRequestManager(this); | 362 | m_rayCastManager = new ODERayCastRequestManager(this); |
363 | |||
537 | 364 | ||
538 | // Create the world and the first space | 365 | lock (OdeLock) |
539 | world = d.WorldCreate(); | 366 | { |
540 | space = d.HashSpaceCreate(IntPtr.Zero); | 367 | // Create the world and the first space |
368 | try | ||
369 | { | ||
370 | world = d.WorldCreate(); | ||
371 | TopSpace = d.HashSpaceCreate(IntPtr.Zero); | ||
541 | 372 | ||
542 | contactgroup = d.JointGroupCreate(0); | 373 | // now the major subspaces |
374 | ActiveSpace = d.HashSpaceCreate(TopSpace); | ||
375 | StaticSpace = d.HashSpaceCreate(TopSpace); | ||
376 | GroundSpace = d.HashSpaceCreate(TopSpace); | ||
377 | } | ||
378 | catch | ||
379 | { | ||
380 | // i must RtC#FM | ||
381 | } | ||
543 | 382 | ||
544 | d.WorldSetAutoDisableFlag(world, false); | 383 | d.HashSpaceSetLevels(TopSpace, -2, 8); |
384 | d.HashSpaceSetLevels(ActiveSpace, -2, 8); | ||
385 | d.HashSpaceSetLevels(StaticSpace, -2, 8); | ||
386 | d.HashSpaceSetLevels(GroundSpace, 0, 8); | ||
545 | 387 | ||
546 | #if USE_DRAWSTUFF | 388 | // demote to second level |
547 | Thread viewthread = new Thread(new ParameterizedThreadStart(startvisualization)); | 389 | d.SpaceSetSublevel(ActiveSpace, 1); |
548 | viewthread.Start(); | 390 | d.SpaceSetSublevel(StaticSpace, 1); |
549 | #endif | 391 | d.SpaceSetSublevel(GroundSpace, 1); |
550 | 392 | ||
551 | _watermap = new float[258 * 258]; | 393 | d.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space | |
394 | CollisionCategories.Geom | | ||
395 | CollisionCategories.Character | | ||
396 | CollisionCategories.Phantom | | ||
397 | CollisionCategories.VolumeDtc | ||
398 | )); | ||
399 | d.GeomSetCollideBits(ActiveSpace, 0); | ||
400 | d.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space | | ||
401 | CollisionCategories.Geom | | ||
402 | CollisionCategories.Land | | ||
403 | CollisionCategories.Water | | ||
404 | CollisionCategories.Phantom | | ||
405 | CollisionCategories.VolumeDtc | ||
406 | )); | ||
407 | d.GeomSetCollideBits(StaticSpace, 0); | ||
552 | 408 | ||
553 | // Zero out the prim spaces array (we split our space into smaller spaces so | 409 | d.GeomSetCategoryBits(GroundSpace, (uint)(CollisionCategories.Land)); |
554 | // we can hit test less. | 410 | d.GeomSetCollideBits(GroundSpace, 0); |
555 | } | ||
556 | 411 | ||
557 | #if USE_DRAWSTUFF | 412 | contactgroup = d.JointGroupCreate(0); |
558 | public void startvisualization(object o) | 413 | //contactgroup |
559 | { | 414 | |
560 | ds.Functions fn; | 415 | d.WorldSetAutoDisableFlag(world, false); |
561 | fn.version = ds.VERSION; | 416 | } |
562 | fn.start = new ds.CallbackFunction(start); | ||
563 | fn.step = new ds.CallbackFunction(step); | ||
564 | fn.command = new ds.CallbackFunction(command); | ||
565 | fn.stop = null; | ||
566 | fn.path_to_textures = "./textures"; | ||
567 | string[] args = new string[0]; | ||
568 | ds.SimulationLoop(args.Length, args, 352, 288, ref fn); | ||
569 | } | 417 | } |
570 | #endif | ||
571 | 418 | ||
572 | // Initialize the mesh plugin | 419 | // Initialize the mesh plugin |
420 | // public override void Initialise(IMesher meshmerizer, IConfigSource config, RegionInfo region ) | ||
573 | public override void Initialise(IMesher meshmerizer, IConfigSource config) | 421 | public override void Initialise(IMesher meshmerizer, IConfigSource config) |
574 | { | 422 | { |
575 | InitializeExtraStats(); | 423 | // checkThread(); |
576 | |||
577 | mesher = meshmerizer; | 424 | mesher = meshmerizer; |
578 | m_config = config; | 425 | m_config = config; |
579 | // Defaults | ||
580 | 426 | ||
581 | if (Environment.OSVersion.Platform == PlatformID.Unix) | 427 | string ode_config = d.GetConfiguration(); |
428 | if (ode_config != null && ode_config != "") | ||
582 | { | 429 | { |
583 | avPIDD = 3200.0f; | 430 | m_log.WarnFormat("ODE configuration: {0}", ode_config); |
584 | avPIDP = 1400.0f; | 431 | |
585 | avStandupTensor = 2000000f; | 432 | if (ode_config.Contains("ODE_Ubit")) |
586 | } | 433 | { |
587 | else | 434 | OdeUbitLib = true; |
588 | { | 435 | } |
589 | avPIDD = 2200.0f; | ||
590 | avPIDP = 900.0f; | ||
591 | avStandupTensor = 550000f; | ||
592 | } | 436 | } |
593 | 437 | ||
438 | /* | ||
439 | if (region != null) | ||
440 | { | ||
441 | WorldExtents.X = region.RegionSizeX; | ||
442 | WorldExtents.Y = region.RegionSizeY; | ||
443 | } | ||
444 | */ | ||
445 | |||
446 | // Defaults | ||
447 | |||
594 | int contactsPerCollision = 80; | 448 | int contactsPerCollision = 80; |
595 | 449 | ||
450 | IConfig physicsconfig = null; | ||
451 | |||
596 | if (m_config != null) | 452 | if (m_config != null) |
597 | { | 453 | { |
598 | IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"]; | 454 | physicsconfig = m_config.Configs["ODEPhysicsSettings"]; |
599 | if (physicsconfig != null) | 455 | if (physicsconfig != null) |
600 | { | 456 | { |
601 | CollectStats = physicsconfig.GetBoolean("collect_stats", false); | 457 | gravityx = physicsconfig.GetFloat("world_gravityx", gravityx); |
602 | 458 | gravityy = physicsconfig.GetFloat("world_gravityy", gravityy); | |
603 | gravityx = physicsconfig.GetFloat("world_gravityx", 0f); | 459 | gravityz = physicsconfig.GetFloat("world_gravityz", gravityz); |
604 | gravityy = physicsconfig.GetFloat("world_gravityy", 0f); | ||
605 | gravityz = physicsconfig.GetFloat("world_gravityz", -9.8f); | ||
606 | |||
607 | float avatarTerminalVelocity = physicsconfig.GetFloat("avatar_terminal_velocity", 54f); | ||
608 | AvatarTerminalVelocity = Util.Clamp<float>(avatarTerminalVelocity, 0, 255f); | ||
609 | if (AvatarTerminalVelocity != avatarTerminalVelocity) | ||
610 | { | ||
611 | m_log.WarnFormat( | ||
612 | "[ODE SCENE]: avatar_terminal_velocity of {0} is invalid. Clamping to {1}", | ||
613 | avatarTerminalVelocity, AvatarTerminalVelocity); | ||
614 | } | ||
615 | |||
616 | worldHashspaceLow = physicsconfig.GetInt("world_hashspace_size_low", -4); | ||
617 | worldHashspaceHigh = physicsconfig.GetInt("world_hashspace_size_high", 128); | ||
618 | |||
619 | metersInSpace = physicsconfig.GetFloat("meters_in_small_space", 29.9f); | ||
620 | smallHashspaceLow = physicsconfig.GetInt("small_hashspace_size_low", -4); | ||
621 | smallHashspaceHigh = physicsconfig.GetInt("small_hashspace_size_high", 66); | ||
622 | |||
623 | contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", 0.001f); | ||
624 | |||
625 | nmTerrainContactFriction = physicsconfig.GetFloat("nm_terraincontact_friction", 255.0f); | ||
626 | nmTerrainContactBounce = physicsconfig.GetFloat("nm_terraincontact_bounce", 0.1f); | ||
627 | nmTerrainContactERP = physicsconfig.GetFloat("nm_terraincontact_erp", 0.1025f); | ||
628 | 460 | ||
629 | mTerrainContactFriction = physicsconfig.GetFloat("m_terraincontact_friction", 75f); | 461 | metersInSpace = physicsconfig.GetFloat("meters_in_small_space", metersInSpace); |
630 | mTerrainContactBounce = physicsconfig.GetFloat("m_terraincontact_bounce", 0.05f); | ||
631 | mTerrainContactERP = physicsconfig.GetFloat("m_terraincontact_erp", 0.05025f); | ||
632 | 462 | ||
633 | nmAvatarObjectContactFriction = physicsconfig.GetFloat("objectcontact_friction", 250f); | 463 | contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer); |
634 | nmAvatarObjectContactBounce = physicsconfig.GetFloat("objectcontact_bounce", 0.2f); | ||
635 | |||
636 | mAvatarObjectContactFriction = physicsconfig.GetFloat("m_avatarobjectcontact_friction", 75f); | ||
637 | mAvatarObjectContactBounce = physicsconfig.GetFloat("m_avatarobjectcontact_bounce", 0.1f); | ||
638 | 464 | ||
639 | ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE); | 465 | ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE); |
640 | m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", 10); | 466 | m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", m_physicsiterations); |
641 | 467 | ||
642 | avDensity = physicsconfig.GetFloat("av_density", 80f); | 468 | avDensity = physicsconfig.GetFloat("av_density", avDensity); |
643 | // avHeightFudgeFactor = physicsconfig.GetFloat("av_height_fudge_factor", 0.52f); | 469 | avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk); |
644 | avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", 1.3f); | 470 | avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun); |
645 | avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", 0.8f); | 471 | avCapRadius = physicsconfig.GetFloat("av_capsule_radius", avCapRadius); |
646 | avCapRadius = physicsconfig.GetFloat("av_capsule_radius", 0.37f); | ||
647 | IsAvCapsuleTilted = physicsconfig.GetBoolean("av_capsule_tilted", false); | ||
648 | 472 | ||
649 | contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", 80); | 473 | contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision); |
650 | 474 | ||
651 | geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 5); | 475 | geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3); |
652 | geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15); | 476 | geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15); |
653 | geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5); | 477 | // geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5); |
654 | |||
655 | geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", 10.000006836f); | ||
656 | bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20); | ||
657 | |||
658 | bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", 35f); | ||
659 | bodyPIDG = physicsconfig.GetFloat("body_pid_gain", 25f); | ||
660 | 478 | ||
661 | forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing); | 479 | geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity); |
662 | meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", true); | 480 | bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable); |
663 | meshSculptLOD = physicsconfig.GetFloat("mesh_lod", 32f); | ||
664 | MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", 16f); | ||
665 | m_filterCollisions = physicsconfig.GetBoolean("filter_collisions", false); | ||
666 | |||
667 | if (Environment.OSVersion.Platform == PlatformID.Unix) | ||
668 | { | ||
669 | avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", 2200.0f); | ||
670 | avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", 900.0f); | ||
671 | avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_linux", 550000f); | ||
672 | bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_linux", 5f); | ||
673 | } | ||
674 | else | ||
675 | { | ||
676 | avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", 2200.0f); | ||
677 | avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", 900.0f); | ||
678 | avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_win", 550000f); | ||
679 | bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_win", 5f); | ||
680 | } | ||
681 | 481 | ||
682 | physics_logging = physicsconfig.GetBoolean("physics_logging", false); | 482 | physics_logging = physicsconfig.GetBoolean("physics_logging", false); |
683 | physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0); | 483 | physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0); |
684 | physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false); | 484 | physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false); |
685 | 485 | ||
686 | m_NINJA_physics_joints_enabled = physicsconfig.GetBoolean("use_NINJA_physics_joints", false); | 486 | minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", minimumGroundFlightOffset); |
687 | minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", 3f); | 487 | maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", maximumMassObject); |
688 | maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", 10000.01f); | ||
689 | } | 488 | } |
690 | } | 489 | } |
691 | 490 | ||
692 | contacts = new d.ContactGeom[contactsPerCollision]; | 491 | m_meshWorker = new ODEMeshWorker(this, m_log, meshmerizer, physicsconfig); |
693 | |||
694 | staticPrimspace = new IntPtr[(int)(300 / metersInSpace), (int)(300 / metersInSpace)]; | ||
695 | |||
696 | // Centeral contact friction and bounce | ||
697 | // ckrinke 11/10/08 Enabling soft_erp but not soft_cfm until I figure out why | ||
698 | // an avatar falls through in Z but not in X or Y when walking on a prim. | ||
699 | contact.surface.mode |= d.ContactFlags.SoftERP; | ||
700 | contact.surface.mu = nmAvatarObjectContactFriction; | ||
701 | contact.surface.bounce = nmAvatarObjectContactBounce; | ||
702 | contact.surface.soft_cfm = 0.010f; | ||
703 | contact.surface.soft_erp = 0.010f; | ||
704 | |||
705 | // Terrain contact friction and Bounce | ||
706 | // This is the *non* moving version. Use this when an avatar | ||
707 | // isn't moving to keep it in place better | ||
708 | TerrainContact.surface.mode |= d.ContactFlags.SoftERP; | ||
709 | TerrainContact.surface.mu = nmTerrainContactFriction; | ||
710 | TerrainContact.surface.bounce = nmTerrainContactBounce; | ||
711 | TerrainContact.surface.soft_erp = nmTerrainContactERP; | ||
712 | |||
713 | WaterContact.surface.mode |= (d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM); | ||
714 | WaterContact.surface.mu = 0f; // No friction | ||
715 | WaterContact.surface.bounce = 0.0f; // No bounce | ||
716 | WaterContact.surface.soft_cfm = 0.010f; | ||
717 | WaterContact.surface.soft_erp = 0.010f; | ||
718 | |||
719 | // Prim contact friction and bounce | ||
720 | // THis is the *non* moving version of friction and bounce | ||
721 | // Use this when an avatar comes in contact with a prim | ||
722 | // and is moving | ||
723 | AvatarMovementprimContact.surface.mu = mAvatarObjectContactFriction; | ||
724 | AvatarMovementprimContact.surface.bounce = mAvatarObjectContactBounce; | ||
725 | |||
726 | // Terrain contact friction bounce and various error correcting calculations | ||
727 | // Use this when an avatar is in contact with the terrain and moving. | ||
728 | AvatarMovementTerrainContact.surface.mode |= d.ContactFlags.SoftERP; | ||
729 | AvatarMovementTerrainContact.surface.mu = mTerrainContactFriction; | ||
730 | AvatarMovementTerrainContact.surface.bounce = mTerrainContactBounce; | ||
731 | AvatarMovementTerrainContact.surface.soft_erp = mTerrainContactERP; | ||
732 | 492 | ||
733 | /* | 493 | HalfOdeStep = ODE_STEPSIZE * 0.5f; |
734 | <summary></summary> | 494 | odetimestepMS = (int)(1000.0f * ODE_STEPSIZE +0.5f); |
735 | Stone = 0, | ||
736 | /// <summary></summary> | ||
737 | Metal = 1, | ||
738 | /// <summary></summary> | ||
739 | Glass = 2, | ||
740 | /// <summary></summary> | ||
741 | Wood = 3, | ||
742 | /// <summary></summary> | ||
743 | Flesh = 4, | ||
744 | /// <summary></summary> | ||
745 | Plastic = 5, | ||
746 | /// <summary></summary> | ||
747 | Rubber = 6 | ||
748 | */ | ||
749 | 495 | ||
750 | m_materialContacts = new d.Contact[7,2]; | 496 | ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf); |
751 | 497 | GlobalContactsArray = GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf); | |
752 | m_materialContacts[(int)Material.Stone, 0] = new d.Contact(); | ||
753 | m_materialContacts[(int)Material.Stone, 0].surface.mode |= d.ContactFlags.SoftERP; | ||
754 | m_materialContacts[(int)Material.Stone, 0].surface.mu = nmAvatarObjectContactFriction; | ||
755 | m_materialContacts[(int)Material.Stone, 0].surface.bounce = nmAvatarObjectContactBounce; | ||
756 | m_materialContacts[(int)Material.Stone, 0].surface.soft_cfm = 0.010f; | ||
757 | m_materialContacts[(int)Material.Stone, 0].surface.soft_erp = 0.010f; | ||
758 | |||
759 | m_materialContacts[(int)Material.Stone, 1] = new d.Contact(); | ||
760 | m_materialContacts[(int)Material.Stone, 1].surface.mode |= d.ContactFlags.SoftERP; | ||
761 | m_materialContacts[(int)Material.Stone, 1].surface.mu = mAvatarObjectContactFriction; | ||
762 | m_materialContacts[(int)Material.Stone, 1].surface.bounce = mAvatarObjectContactBounce; | ||
763 | m_materialContacts[(int)Material.Stone, 1].surface.soft_cfm = 0.010f; | ||
764 | m_materialContacts[(int)Material.Stone, 1].surface.soft_erp = 0.010f; | ||
765 | |||
766 | m_materialContacts[(int)Material.Metal, 0] = new d.Contact(); | ||
767 | m_materialContacts[(int)Material.Metal, 0].surface.mode |= d.ContactFlags.SoftERP; | ||
768 | m_materialContacts[(int)Material.Metal, 0].surface.mu = nmAvatarObjectContactFriction; | ||
769 | m_materialContacts[(int)Material.Metal, 0].surface.bounce = nmAvatarObjectContactBounce; | ||
770 | m_materialContacts[(int)Material.Metal, 0].surface.soft_cfm = 0.010f; | ||
771 | m_materialContacts[(int)Material.Metal, 0].surface.soft_erp = 0.010f; | ||
772 | |||
773 | m_materialContacts[(int)Material.Metal, 1] = new d.Contact(); | ||
774 | m_materialContacts[(int)Material.Metal, 1].surface.mode |= d.ContactFlags.SoftERP; | ||
775 | m_materialContacts[(int)Material.Metal, 1].surface.mu = mAvatarObjectContactFriction; | ||
776 | m_materialContacts[(int)Material.Metal, 1].surface.bounce = mAvatarObjectContactBounce; | ||
777 | m_materialContacts[(int)Material.Metal, 1].surface.soft_cfm = 0.010f; | ||
778 | m_materialContacts[(int)Material.Metal, 1].surface.soft_erp = 0.010f; | ||
779 | |||
780 | m_materialContacts[(int)Material.Glass, 0] = new d.Contact(); | ||
781 | m_materialContacts[(int)Material.Glass, 0].surface.mode |= d.ContactFlags.SoftERP; | ||
782 | m_materialContacts[(int)Material.Glass, 0].surface.mu = 1f; | ||
783 | m_materialContacts[(int)Material.Glass, 0].surface.bounce = 0.5f; | ||
784 | m_materialContacts[(int)Material.Glass, 0].surface.soft_cfm = 0.010f; | ||
785 | m_materialContacts[(int)Material.Glass, 0].surface.soft_erp = 0.010f; | ||
786 | 498 | ||
787 | /* | 499 | m_materialContactsData[(int)Material.Stone].mu = 0.8f; |
788 | private float nmAvatarObjectContactFriction = 250f; | 500 | m_materialContactsData[(int)Material.Stone].bounce = 0.4f; |
789 | private float nmAvatarObjectContactBounce = 0.1f; | ||
790 | 501 | ||
791 | private float mAvatarObjectContactFriction = 75f; | 502 | m_materialContactsData[(int)Material.Metal].mu = 0.3f; |
792 | private float mAvatarObjectContactBounce = 0.1f; | 503 | m_materialContactsData[(int)Material.Metal].bounce = 0.4f; |
793 | */ | 504 | |
794 | m_materialContacts[(int)Material.Glass, 1] = new d.Contact(); | 505 | m_materialContactsData[(int)Material.Glass].mu = 0.2f; |
795 | m_materialContacts[(int)Material.Glass, 1].surface.mode |= d.ContactFlags.SoftERP; | 506 | m_materialContactsData[(int)Material.Glass].bounce = 0.7f; |
796 | m_materialContacts[(int)Material.Glass, 1].surface.mu = 1f; | 507 | |
797 | m_materialContacts[(int)Material.Glass, 1].surface.bounce = 0.5f; | 508 | m_materialContactsData[(int)Material.Wood].mu = 0.6f; |
798 | m_materialContacts[(int)Material.Glass, 1].surface.soft_cfm = 0.010f; | 509 | m_materialContactsData[(int)Material.Wood].bounce = 0.5f; |
799 | m_materialContacts[(int)Material.Glass, 1].surface.soft_erp = 0.010f; | 510 | |
800 | 511 | m_materialContactsData[(int)Material.Flesh].mu = 0.9f; | |
801 | m_materialContacts[(int)Material.Wood, 0] = new d.Contact(); | 512 | m_materialContactsData[(int)Material.Flesh].bounce = 0.3f; |
802 | m_materialContacts[(int)Material.Wood, 0].surface.mode |= d.ContactFlags.SoftERP; | 513 | |
803 | m_materialContacts[(int)Material.Wood, 0].surface.mu = nmAvatarObjectContactFriction; | 514 | m_materialContactsData[(int)Material.Plastic].mu = 0.4f; |
804 | m_materialContacts[(int)Material.Wood, 0].surface.bounce = nmAvatarObjectContactBounce; | 515 | m_materialContactsData[(int)Material.Plastic].bounce = 0.7f; |
805 | m_materialContacts[(int)Material.Wood, 0].surface.soft_cfm = 0.010f; | 516 | |
806 | m_materialContacts[(int)Material.Wood, 0].surface.soft_erp = 0.010f; | 517 | m_materialContactsData[(int)Material.Rubber].mu = 0.9f; |
807 | 518 | m_materialContactsData[(int)Material.Rubber].bounce = 0.95f; | |
808 | m_materialContacts[(int)Material.Wood, 1] = new d.Contact(); | 519 | |
809 | m_materialContacts[(int)Material.Wood, 1].surface.mode |= d.ContactFlags.SoftERP; | 520 | m_materialContactsData[(int)Material.light].mu = 0.0f; |
810 | m_materialContacts[(int)Material.Wood, 1].surface.mu = mAvatarObjectContactFriction; | 521 | m_materialContactsData[(int)Material.light].bounce = 0.0f; |
811 | m_materialContacts[(int)Material.Wood, 1].surface.bounce = mAvatarObjectContactBounce; | ||
812 | m_materialContacts[(int)Material.Wood, 1].surface.soft_cfm = 0.010f; | ||
813 | m_materialContacts[(int)Material.Wood, 1].surface.soft_erp = 0.010f; | ||
814 | |||
815 | m_materialContacts[(int)Material.Flesh, 0] = new d.Contact(); | ||
816 | m_materialContacts[(int)Material.Flesh, 0].surface.mode |= d.ContactFlags.SoftERP; | ||
817 | m_materialContacts[(int)Material.Flesh, 0].surface.mu = nmAvatarObjectContactFriction; | ||
818 | m_materialContacts[(int)Material.Flesh, 0].surface.bounce = nmAvatarObjectContactBounce; | ||
819 | m_materialContacts[(int)Material.Flesh, 0].surface.soft_cfm = 0.010f; | ||
820 | m_materialContacts[(int)Material.Flesh, 0].surface.soft_erp = 0.010f; | ||
821 | |||
822 | m_materialContacts[(int)Material.Flesh, 1] = new d.Contact(); | ||
823 | m_materialContacts[(int)Material.Flesh, 1].surface.mode |= d.ContactFlags.SoftERP; | ||
824 | m_materialContacts[(int)Material.Flesh, 1].surface.mu = mAvatarObjectContactFriction; | ||
825 | m_materialContacts[(int)Material.Flesh, 1].surface.bounce = mAvatarObjectContactBounce; | ||
826 | m_materialContacts[(int)Material.Flesh, 1].surface.soft_cfm = 0.010f; | ||
827 | m_materialContacts[(int)Material.Flesh, 1].surface.soft_erp = 0.010f; | ||
828 | |||
829 | m_materialContacts[(int)Material.Plastic, 0] = new d.Contact(); | ||
830 | m_materialContacts[(int)Material.Plastic, 0].surface.mode |= d.ContactFlags.SoftERP; | ||
831 | m_materialContacts[(int)Material.Plastic, 0].surface.mu = nmAvatarObjectContactFriction; | ||
832 | m_materialContacts[(int)Material.Plastic, 0].surface.bounce = nmAvatarObjectContactBounce; | ||
833 | m_materialContacts[(int)Material.Plastic, 0].surface.soft_cfm = 0.010f; | ||
834 | m_materialContacts[(int)Material.Plastic, 0].surface.soft_erp = 0.010f; | ||
835 | |||
836 | m_materialContacts[(int)Material.Plastic, 1] = new d.Contact(); | ||
837 | m_materialContacts[(int)Material.Plastic, 1].surface.mode |= d.ContactFlags.SoftERP; | ||
838 | m_materialContacts[(int)Material.Plastic, 1].surface.mu = mAvatarObjectContactFriction; | ||
839 | m_materialContacts[(int)Material.Plastic, 1].surface.bounce = mAvatarObjectContactBounce; | ||
840 | m_materialContacts[(int)Material.Plastic, 1].surface.soft_cfm = 0.010f; | ||
841 | m_materialContacts[(int)Material.Plastic, 1].surface.soft_erp = 0.010f; | ||
842 | |||
843 | m_materialContacts[(int)Material.Rubber, 0] = new d.Contact(); | ||
844 | m_materialContacts[(int)Material.Rubber, 0].surface.mode |= d.ContactFlags.SoftERP; | ||
845 | m_materialContacts[(int)Material.Rubber, 0].surface.mu = nmAvatarObjectContactFriction; | ||
846 | m_materialContacts[(int)Material.Rubber, 0].surface.bounce = nmAvatarObjectContactBounce; | ||
847 | m_materialContacts[(int)Material.Rubber, 0].surface.soft_cfm = 0.010f; | ||
848 | m_materialContacts[(int)Material.Rubber, 0].surface.soft_erp = 0.010f; | ||
849 | |||
850 | m_materialContacts[(int)Material.Rubber, 1] = new d.Contact(); | ||
851 | m_materialContacts[(int)Material.Rubber, 1].surface.mode |= d.ContactFlags.SoftERP; | ||
852 | m_materialContacts[(int)Material.Rubber, 1].surface.mu = mAvatarObjectContactFriction; | ||
853 | m_materialContacts[(int)Material.Rubber, 1].surface.bounce = mAvatarObjectContactBounce; | ||
854 | m_materialContacts[(int)Material.Rubber, 1].surface.soft_cfm = 0.010f; | ||
855 | m_materialContacts[(int)Material.Rubber, 1].surface.soft_erp = 0.010f; | ||
856 | |||
857 | d.HashSpaceSetLevels(space, worldHashspaceLow, worldHashspaceHigh); | ||
858 | 522 | ||
859 | // Set the gravity,, don't disable things automatically (we set it explicitly on some things) | 523 | // Set the gravity,, don't disable things automatically (we set it explicitly on some things) |
860 | 524 | ||
861 | d.WorldSetGravity(world, gravityx, gravityy, gravityz); | 525 | d.WorldSetGravity(world, gravityx, gravityy, gravityz); |
862 | d.WorldSetContactSurfaceLayer(world, contactsurfacelayer); | 526 | d.WorldSetContactSurfaceLayer(world, contactsurfacelayer); |
863 | 527 | ||
864 | d.WorldSetLinearDamping(world, 256f); | 528 | d.WorldSetLinearDamping(world, 0.002f); |
865 | d.WorldSetAngularDamping(world, 256f); | 529 | d.WorldSetAngularDamping(world, 0.002f); |
866 | d.WorldSetAngularDampingThreshold(world, 256f); | 530 | d.WorldSetAngularDampingThreshold(world, 0f); |
867 | d.WorldSetLinearDampingThreshold(world, 256f); | 531 | d.WorldSetLinearDampingThreshold(world, 0f); |
868 | d.WorldSetMaxAngularSpeed(world, 256f); | 532 | d.WorldSetMaxAngularSpeed(world, 100f); |
533 | |||
534 | d.WorldSetCFM(world,1e-6f); // a bit harder than default | ||
535 | //d.WorldSetCFM(world, 1e-4f); // a bit harder than default | ||
536 | d.WorldSetERP(world, 0.6f); // higher than original | ||
869 | 537 | ||
870 | // Set how many steps we go without running collision testing | 538 | // Set how many steps we go without running collision testing |
871 | // This is in addition to the step size. | 539 | // This is in addition to the step size. |
872 | // Essentially Steps * m_physicsiterations | 540 | // Essentially Steps * m_physicsiterations |
873 | d.WorldSetQuickStepNumIterations(world, m_physicsiterations); | 541 | d.WorldSetQuickStepNumIterations(world, m_physicsiterations); |
874 | //d.WorldSetContactMaxCorrectingVel(world, 1000.0f); | ||
875 | 542 | ||
876 | for (int i = 0; i < staticPrimspace.GetLength(0); i++) | 543 | d.WorldSetContactMaxCorrectingVel(world, 60.0f); |
877 | { | 544 | |
878 | for (int j = 0; j < staticPrimspace.GetLength(1); j++) | 545 | spacesPerMeter = 1 / metersInSpace; |
546 | spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeter); | ||
547 | spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeter); | ||
548 | |||
549 | staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY]; | ||
550 | |||
551 | // create all spaces now | ||
552 | int i, j; | ||
553 | IntPtr newspace; | ||
554 | |||
555 | for (i = 0; i < spaceGridMaxX; i++) | ||
556 | for (j = 0; j < spaceGridMaxY; j++) | ||
879 | { | 557 | { |
880 | staticPrimspace[i, j] = IntPtr.Zero; | 558 | newspace = d.HashSpaceCreate(StaticSpace); |
559 | d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space); | ||
560 | waitForSpaceUnlock(newspace); | ||
561 | d.SpaceSetSublevel(newspace, 2); | ||
562 | d.HashSpaceSetLevels(newspace, -2, 8); | ||
563 | d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space | | ||
564 | CollisionCategories.Geom | | ||
565 | CollisionCategories.Land | | ||
566 | CollisionCategories.Water | | ||
567 | CollisionCategories.Phantom | | ||
568 | CollisionCategories.VolumeDtc | ||
569 | )); | ||
570 | d.GeomSetCollideBits(newspace, 0); | ||
571 | |||
572 | staticPrimspace[i, j] = newspace; | ||
573 | } | ||
574 | // let this now be real maximum values | ||
575 | spaceGridMaxX--; | ||
576 | spaceGridMaxY--; | ||
577 | |||
578 | // create 4 off world spaces (x<0,x>max,y<0,y>max) | ||
579 | staticPrimspaceOffRegion = new IntPtr[4]; | ||
580 | |||
581 | for (i = 0; i < 4; i++) | ||
582 | { | ||
583 | newspace = d.HashSpaceCreate(StaticSpace); | ||
584 | d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space); | ||
585 | waitForSpaceUnlock(newspace); | ||
586 | d.SpaceSetSublevel(newspace, 2); | ||
587 | d.HashSpaceSetLevels(newspace, -2, 8); | ||
588 | d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space | | ||
589 | CollisionCategories.Geom | | ||
590 | CollisionCategories.Land | | ||
591 | CollisionCategories.Water | | ||
592 | CollisionCategories.Phantom | | ||
593 | CollisionCategories.VolumeDtc | ||
594 | )); | ||
595 | d.GeomSetCollideBits(newspace, 0); | ||
596 | |||
597 | staticPrimspaceOffRegion[i] = newspace; | ||
881 | } | 598 | } |
882 | } | ||
883 | 599 | ||
884 | _worldInitialized = true; | 600 | m_lastframe = DateTime.UtcNow; |
601 | m_lastMeshExpire = m_lastframe; | ||
885 | } | 602 | } |
886 | 603 | ||
887 | // internal void waitForSpaceUnlock(IntPtr space) | 604 | internal void waitForSpaceUnlock(IntPtr space) |
888 | // { | 605 | { |
889 | // //if (space != IntPtr.Zero) | 606 | //if (space != IntPtr.Zero) |
890 | // //while (d.SpaceLockQuery(space)) { } // Wait and do nothing | 607 | //while (d.SpaceLockQuery(space)) { } // Wait and do nothing |
891 | // } | 608 | } |
892 | |||
893 | // /// <summary> | ||
894 | // /// Debug space message for printing the space that a prim/avatar is in. | ||
895 | // /// </summary> | ||
896 | // /// <param name="pos"></param> | ||
897 | // /// <returns>Returns which split up space the given position is in.</returns> | ||
898 | // public string whichspaceamIin(Vector3 pos) | ||
899 | // { | ||
900 | // return calculateSpaceForGeom(pos).ToString(); | ||
901 | // } | ||
902 | 609 | ||
903 | #region Collision Detection | 610 | #region Collision Detection |
904 | 611 | ||
905 | /// <summary> | 612 | // sets a global contact for a joint for contactgeom , and base contact description) |
906 | /// Collides two geometries. | 613 | |
907 | /// </summary> | 614 | private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom, float mu, float bounce, float cfm, float erpscale, float dscale) |
908 | /// <returns></returns> | ||
909 | /// <param name='geom1'></param> | ||
910 | /// <param name='geom2'>/param> | ||
911 | /// <param name='maxContacts'></param> | ||
912 | /// <param name='contactsArray'></param> | ||
913 | /// <param name='contactGeomSize'></param> | ||
914 | private int CollideGeoms( | ||
915 | IntPtr geom1, IntPtr geom2, int maxContacts, Ode.NET.d.ContactGeom[] contactsArray, int contactGeomSize) | ||
916 | { | 615 | { |
917 | int count; | 616 | if (GlobalContactsArray == IntPtr.Zero || m_global_contactcount >= maxContactsbeforedeath) |
617 | return IntPtr.Zero; | ||
918 | 618 | ||
919 | lock (OdeScene.UniversalColliderSyncObject) | 619 | float erp = contactGeom.depth; |
920 | { | 620 | erp *= erpscale; |
921 | // We do this inside the lock so that we don't count any delay in acquiring it | 621 | if (erp < minERP) |
922 | if (CollectStats) | 622 | erp = minERP; |
923 | m_nativeCollisionStartTick = Util.EnvironmentTickCount(); | 623 | else if (erp > MaxERP) |
624 | erp = MaxERP; | ||
924 | 625 | ||
925 | count = d.Collide(geom1, geom2, maxContacts, contactsArray, contactGeomSize); | 626 | float depth = contactGeom.depth * dscale; |
926 | } | 627 | if (depth > 0.5f) |
628 | depth = 0.5f; | ||
629 | |||
630 | d.Contact newcontact = new d.Contact(); | ||
631 | newcontact.geom.depth = depth; | ||
632 | newcontact.geom.g1 = contactGeom.g1; | ||
633 | newcontact.geom.g2 = contactGeom.g2; | ||
634 | newcontact.geom.pos = contactGeom.pos; | ||
635 | newcontact.geom.normal = contactGeom.normal; | ||
636 | newcontact.geom.side1 = contactGeom.side1; | ||
637 | newcontact.geom.side2 = contactGeom.side2; | ||
927 | 638 | ||
928 | // We do this outside the lock so that any waiting threads aren't held up, though the effect is probably | 639 | // this needs bounce also |
929 | // negligable | 640 | newcontact.surface.mode = comumContactFlags; |
930 | if (CollectStats) | 641 | newcontact.surface.mu = mu; |
931 | m_stats[ODENativeGeomCollisionFrameMsStatName] | 642 | newcontact.surface.bounce = bounce; |
932 | += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick); | 643 | newcontact.surface.soft_cfm = cfm; |
644 | newcontact.surface.soft_erp = erp; | ||
933 | 645 | ||
934 | return count; | 646 | IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf)); |
647 | Marshal.StructureToPtr(newcontact, contact, true); | ||
648 | return d.JointCreateContactPtr(world, contactgroup, contact); | ||
935 | } | 649 | } |
936 | 650 | ||
937 | /// <summary> | 651 | private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom) |
938 | /// Collide two spaces or a space and a geometry. | ||
939 | /// </summary> | ||
940 | /// <param name='space1'></param> | ||
941 | /// <param name='space2'>/param> | ||
942 | /// <param name='data'></param> | ||
943 | private void CollideSpaces(IntPtr space1, IntPtr space2, IntPtr data) | ||
944 | { | 652 | { |
945 | if (CollectStats) | 653 | if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision) |
946 | { | 654 | return false; |
947 | m_inCollisionTiming = true; | ||
948 | m_nativeCollisionStartTick = Util.EnvironmentTickCount(); | ||
949 | } | ||
950 | |||
951 | d.SpaceCollide2(space1, space2, data, nearCallback); | ||
952 | 655 | ||
953 | if (CollectStats && m_inCollisionTiming) | 656 | IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf)); |
954 | { | 657 | newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom)); |
955 | m_stats[ODENativeSpaceCollisionFrameMsStatName] | 658 | return true; |
956 | += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick); | ||
957 | m_inCollisionTiming = false; | ||
958 | } | ||
959 | } | 659 | } |
960 | 660 | ||
961 | /// <summary> | 661 | /// <summary> |
@@ -964,76 +664,50 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
964 | /// <param name="space">The space that contains the geoms. Remember, spaces are also geoms</param> | 664 | /// <param name="space">The space that contains the geoms. Remember, spaces are also geoms</param> |
965 | /// <param name="g1">a geometry or space</param> | 665 | /// <param name="g1">a geometry or space</param> |
966 | /// <param name="g2">another geometry or space</param> | 666 | /// <param name="g2">another geometry or space</param> |
667 | /// | ||
668 | |||
967 | private void near(IntPtr space, IntPtr g1, IntPtr g2) | 669 | private void near(IntPtr space, IntPtr g1, IntPtr g2) |
968 | { | 670 | { |
969 | if (CollectStats && m_inCollisionTiming) | ||
970 | { | ||
971 | m_stats[ODENativeSpaceCollisionFrameMsStatName] | ||
972 | += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick); | ||
973 | m_inCollisionTiming = false; | ||
974 | } | ||
975 | |||
976 | // m_log.DebugFormat("[PHYSICS]: Colliding {0} and {1} in {2}", g1, g2, space); | ||
977 | // no lock here! It's invoked from within Simulate(), which is thread-locked | 671 | // no lock here! It's invoked from within Simulate(), which is thread-locked |
978 | 672 | ||
673 | if (m_global_contactcount >= maxContactsbeforedeath) | ||
674 | return; | ||
675 | |||
979 | // Test if we're colliding a geom with a space. | 676 | // Test if we're colliding a geom with a space. |
980 | // If so we have to drill down into the space recursively | 677 | // If so we have to drill down into the space recursively |
981 | 678 | ||
679 | if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) | ||
680 | return; | ||
681 | |||
982 | if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2)) | 682 | if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2)) |
983 | { | 683 | { |
984 | if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) | ||
985 | return; | ||
986 | |||
987 | // Separating static prim geometry spaces. | ||
988 | // We'll be calling near recursivly if one | 684 | // We'll be calling near recursivly if one |
989 | // of them is a space to find all of the | 685 | // of them is a space to find all of the |
990 | // contact points in the space | 686 | // contact points in the space |
991 | try | 687 | try |
992 | { | 688 | { |
993 | CollideSpaces(g1, g2, IntPtr.Zero); | 689 | d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback); |
994 | } | 690 | } |
995 | catch (AccessViolationException) | 691 | catch (AccessViolationException) |
996 | { | 692 | { |
997 | m_log.Error("[ODE SCENE]: Unable to collide test a space"); | 693 | m_log.Warn("[PHYSICS]: Unable to collide test a space"); |
998 | return; | 694 | return; |
999 | } | 695 | } |
1000 | //Colliding a space or a geom with a space or a geom. so drill down | 696 | //here one should check collisions of geoms inside a space |
1001 | 697 | // but on each space we only should have geoms that not colide amoung each other | |
1002 | //Collide all geoms in each space.. | 698 | // so we don't dig inside spaces |
1003 | //if (d.GeomIsSpace(g1)) d.SpaceCollide(g1, IntPtr.Zero, nearCallback); | ||
1004 | //if (d.GeomIsSpace(g2)) d.SpaceCollide(g2, IntPtr.Zero, nearCallback); | ||
1005 | return; | 699 | return; |
1006 | } | 700 | } |
1007 | 701 | ||
1008 | if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) | 702 | // get geom bodies to check if we already a joint contact |
1009 | return; | 703 | // guess this shouldn't happen now |
1010 | |||
1011 | IntPtr b1 = d.GeomGetBody(g1); | 704 | IntPtr b1 = d.GeomGetBody(g1); |
1012 | IntPtr b2 = d.GeomGetBody(g2); | 705 | IntPtr b2 = d.GeomGetBody(g2); |
1013 | 706 | ||
1014 | // d.GeomClassID id = d.GeomGetClass(g1); | 707 | // d.GeomClassID id = d.GeomGetClass(g1); |
1015 | 708 | ||
1016 | String name1 = null; | ||
1017 | String name2 = null; | ||
1018 | |||
1019 | if (!geom_name_map.TryGetValue(g1, out name1)) | ||
1020 | { | ||
1021 | name1 = "null"; | ||
1022 | } | ||
1023 | if (!geom_name_map.TryGetValue(g2, out name2)) | ||
1024 | { | ||
1025 | name2 = "null"; | ||
1026 | } | ||
1027 | |||
1028 | //if (id == d.GeomClassId.TriMeshClass) | ||
1029 | //{ | ||
1030 | // m_log.InfoFormat("near: A collision was detected between {1} and {2}", 0, name1, name2); | ||
1031 | //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2); | ||
1032 | //} | ||
1033 | |||
1034 | // Figure out how many contact points we have | 709 | // Figure out how many contact points we have |
1035 | int count = 0; | 710 | int count = 0; |
1036 | |||
1037 | try | 711 | try |
1038 | { | 712 | { |
1039 | // Colliding Geom To Geom | 713 | // Colliding Geom To Geom |
@@ -1045,914 +719,611 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1045 | if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact)) | 719 | if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact)) |
1046 | return; | 720 | return; |
1047 | 721 | ||
1048 | count = CollideGeoms(g1, g2, contacts.Length, contacts, d.ContactGeom.SizeOf); | 722 | // debug |
723 | PhysicsActor dp2; | ||
724 | if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass) | ||
725 | { | ||
726 | d.AABB aabb; | ||
727 | d.GeomGetAABB(g2, out aabb); | ||
728 | float x = aabb.MaxX - aabb.MinX; | ||
729 | float y = aabb.MaxY - aabb.MinY; | ||
730 | float z = aabb.MaxZ - aabb.MinZ; | ||
731 | if (x > 60.0f || y > 60.0f || z > 60.0f) | ||
732 | { | ||
733 | if (!actor_name_map.TryGetValue(g2, out dp2)) | ||
734 | m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2"); | ||
735 | else | ||
736 | m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})", | ||
737 | dp2.Name, dp2.Size, x, y, z, | ||
738 | dp2.Position.ToString(), | ||
739 | dp2.Orientation.ToString(), | ||
740 | dp2.Orientation.Length()); | ||
741 | return; | ||
742 | } | ||
743 | } | ||
744 | // | ||
745 | |||
1049 | 746 | ||
1050 | // All code after this is only relevant if we have any collisions | ||
1051 | if (count <= 0) | ||
1052 | return; | ||
1053 | 747 | ||
1054 | if (count > contacts.Length) | 748 | if(d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc || |
1055 | m_log.Error("[ODE SCENE]: Got " + count + " contacts when we asked for a maximum of " + contacts.Length); | 749 | d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc) |
750 | { | ||
751 | int cflags; | ||
752 | unchecked | ||
753 | { | ||
754 | cflags = (int)(1 | d.CONTACTS_UNIMPORTANT); | ||
755 | } | ||
756 | count = d.CollidePtr(g1, g2, cflags, ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); | ||
757 | } | ||
758 | else | ||
759 | count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); | ||
1056 | } | 760 | } |
1057 | catch (SEHException) | 761 | catch (SEHException) |
1058 | { | 762 | { |
1059 | m_log.Error( | 763 | m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim."); |
1060 | "[ODE SCENE]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim."); | 764 | // ode.drelease(world); |
1061 | base.TriggerPhysicsBasedRestart(); | 765 | base.TriggerPhysicsBasedRestart(); |
1062 | } | 766 | } |
1063 | catch (Exception e) | 767 | catch (Exception e) |
1064 | { | 768 | { |
1065 | m_log.ErrorFormat("[ODE SCENE]: Unable to collide test an object: {0}", e.Message); | 769 | m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message); |
1066 | return; | 770 | return; |
1067 | } | 771 | } |
1068 | 772 | ||
773 | // contacts done | ||
774 | if (count == 0) | ||
775 | return; | ||
776 | |||
777 | // try get physical actors | ||
1069 | PhysicsActor p1; | 778 | PhysicsActor p1; |
1070 | PhysicsActor p2; | 779 | PhysicsActor p2; |
1071 | 780 | ||
1072 | p1ExpectedPoints = 0; | ||
1073 | p2ExpectedPoints = 0; | ||
1074 | |||
1075 | if (!actor_name_map.TryGetValue(g1, out p1)) | 781 | if (!actor_name_map.TryGetValue(g1, out p1)) |
1076 | { | 782 | { |
1077 | p1 = PANull; | 783 | m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 1"); |
784 | return; | ||
1078 | } | 785 | } |
1079 | 786 | ||
1080 | if (!actor_name_map.TryGetValue(g2, out p2)) | 787 | if (!actor_name_map.TryGetValue(g2, out p2)) |
1081 | { | 788 | { |
1082 | p2 = PANull; | 789 | m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2"); |
790 | return; | ||
1083 | } | 791 | } |
1084 | 792 | ||
1085 | ContactPoint maxDepthContact = new ContactPoint(); | 793 | // update actors collision score |
1086 | if (p1.CollisionScore + count >= float.MaxValue) | 794 | if (p1.CollisionScore >= float.MaxValue - count) |
1087 | p1.CollisionScore = 0; | 795 | p1.CollisionScore = 0; |
1088 | p1.CollisionScore += count; | 796 | p1.CollisionScore += count; |
1089 | 797 | ||
1090 | if (p2.CollisionScore + count >= float.MaxValue) | 798 | if (p2.CollisionScore >= float.MaxValue - count) |
1091 | p2.CollisionScore = 0; | 799 | p2.CollisionScore = 0; |
1092 | p2.CollisionScore += count; | 800 | p2.CollisionScore += count; |
1093 | 801 | ||
1094 | for (int i = 0; i < count; i++) | 802 | // get first contact |
1095 | { | 803 | d.ContactGeom curContact = new d.ContactGeom(); |
1096 | d.ContactGeom curContact = contacts[i]; | 804 | if (!GetCurContactGeom(0, ref curContact)) |
1097 | 805 | return; | |
1098 | if (curContact.depth > maxDepthContact.PenetrationDepth) | 806 | // for now it's the one with max depth |
1099 | { | 807 | ContactPoint maxDepthContact = new ContactPoint( |
1100 | maxDepthContact = new ContactPoint( | ||
1101 | new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z), | 808 | new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z), |
1102 | new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z), | 809 | new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z), |
1103 | curContact.depth | 810 | curContact.depth |
1104 | ); | 811 | ); |
1105 | } | 812 | // do volume detection case |
1106 | 813 | if ( | |
1107 | //m_log.Warn("[CCOUNT]: " + count); | 814 | (p1.IsVolumeDtc || p2.IsVolumeDtc)) |
1108 | IntPtr joint; | 815 | { |
1109 | // If we're colliding with terrain, use 'TerrainContact' instead of contact. | 816 | collision_accounting_events(p1, p2, maxDepthContact); |
1110 | // allows us to have different settings | 817 | return; |
1111 | 818 | } | |
1112 | // We only need to test p2 for 'jump crouch purposes' | ||
1113 | if (p2 is OdeCharacter && p1.PhysicsActorType == (int)ActorTypes.Prim) | ||
1114 | { | ||
1115 | // Testing if the collision is at the feet of the avatar | ||
1116 | 819 | ||
1117 | //m_log.DebugFormat("[PHYSICS]: {0} - {1} - {2} - {3}", curContact.pos.Z, p2.Position.Z, (p2.Position.Z - curContact.pos.Z), (p2.Size.Z * 0.6f)); | 820 | // big messy collision analises |
1118 | if ((p2.Position.Z - curContact.pos.Z) > (p2.Size.Z * 0.6f)) | ||
1119 | p2.IsColliding = true; | ||
1120 | } | ||
1121 | else | ||
1122 | { | ||
1123 | p2.IsColliding = true; | ||
1124 | } | ||
1125 | |||
1126 | //if ((framecount % m_returncollisions) == 0) | ||
1127 | 821 | ||
1128 | switch (p1.PhysicsActorType) | 822 | Vector3 normoverride = Vector3.Zero; //damm c# |
1129 | { | ||
1130 | case (int)ActorTypes.Agent: | ||
1131 | p1ExpectedPoints = avatarExpectedContacts; | ||
1132 | p2.CollidingObj = true; | ||
1133 | break; | ||
1134 | case (int)ActorTypes.Prim: | ||
1135 | if (p1 != null && p1 is OdePrim) | ||
1136 | p1ExpectedPoints = ((OdePrim) p1).ExpectedCollisionContacts; | ||
1137 | 823 | ||
1138 | if (p2.Velocity.LengthSquared() > 0.0f) | 824 | float mu = 0; |
1139 | p2.CollidingObj = true; | 825 | float bounce = 0; |
1140 | break; | 826 | float cfm = 0.0001f; |
1141 | case (int)ActorTypes.Unknown: | 827 | float erpscale = 1.0f; |
1142 | p2.CollidingGround = true; | 828 | float dscale = 1.0f; |
1143 | break; | 829 | bool IgnoreNegSides = false; |
1144 | default: | ||
1145 | p2.CollidingGround = true; | ||
1146 | break; | ||
1147 | } | ||
1148 | 830 | ||
1149 | // we don't want prim or avatar to explode | 831 | ContactData contactdata1 = new ContactData(0, 0, false); |
832 | ContactData contactdata2 = new ContactData(0, 0, false); | ||
1150 | 833 | ||
1151 | #region InterPenetration Handling - Unintended physics explosions | 834 | bool dop1foot = false; |
1152 | # region disabled code1 | 835 | bool dop2foot = false; |
836 | bool ignore = false; | ||
837 | bool AvanormOverride = false; | ||
1153 | 838 | ||
1154 | if (curContact.depth >= 0.08f) | 839 | switch (p1.PhysicsActorType) |
1155 | { | 840 | { |
1156 | //This is disabled at the moment only because it needs more tweaking | 841 | case (int)ActorTypes.Agent: |
1157 | //It will eventually be uncommented | ||
1158 | /* | ||
1159 | if (contact.depth >= 1.00f) | ||
1160 | { | 842 | { |
1161 | //m_log.Debug("[PHYSICS]: " + contact.depth.ToString()); | 843 | AvanormOverride = true; |
1162 | } | 844 | Vector3 tmp = p2.Position - p1.Position; |
845 | normoverride = p2.Velocity - p1.Velocity; | ||
846 | mu = normoverride.LengthSquared(); | ||
1163 | 847 | ||
1164 | //If you interpenetrate a prim with an agent | 848 | if (mu > 1e-6) |
1165 | if ((p2.PhysicsActorType == (int) ActorTypes.Agent && | ||
1166 | p1.PhysicsActorType == (int) ActorTypes.Prim) || | ||
1167 | (p1.PhysicsActorType == (int) ActorTypes.Agent && | ||
1168 | p2.PhysicsActorType == (int) ActorTypes.Prim)) | ||
1169 | { | ||
1170 | |||
1171 | //contact.depth = contact.depth * 4.15f; | ||
1172 | /* | ||
1173 | if (p2.PhysicsActorType == (int) ActorTypes.Agent) | ||
1174 | { | 849 | { |
1175 | p2.CollidingObj = true; | 850 | mu = 1.0f / (float)Math.Sqrt(mu); |
1176 | contact.depth = 0.003f; | 851 | normoverride *= mu; |
1177 | p2.Velocity = p2.Velocity + new PhysicsVector(0, 0, 2.5f); | 852 | mu = Vector3.Dot(tmp, normoverride); |
1178 | OdeCharacter character = (OdeCharacter) p2; | 853 | if (mu > 0) |
1179 | character.SetPidStatus(true); | 854 | normoverride *= -1; |
1180 | contact.pos = new d.Vector3(contact.pos.X + (p1.Size.X / 2), contact.pos.Y + (p1.Size.Y / 2), contact.pos.Z + (p1.Size.Z / 2)); | ||
1181 | |||
1182 | } | 855 | } |
1183 | else | 856 | else |
1184 | { | 857 | { |
1185 | 858 | tmp.Normalize(); | |
1186 | //contact.depth = 0.0000000f; | 859 | normoverride = -tmp; |
1187 | } | 860 | } |
1188 | if (p1.PhysicsActorType == (int) ActorTypes.Agent) | ||
1189 | { | ||
1190 | 861 | ||
1191 | p1.CollidingObj = true; | 862 | switch (p2.PhysicsActorType) |
1192 | contact.depth = 0.003f; | ||
1193 | p1.Velocity = p1.Velocity + new PhysicsVector(0, 0, 2.5f); | ||
1194 | contact.pos = new d.Vector3(contact.pos.X + (p2.Size.X / 2), contact.pos.Y + (p2.Size.Y / 2), contact.pos.Z + (p2.Size.Z / 2)); | ||
1195 | OdeCharacter character = (OdeCharacter)p1; | ||
1196 | character.SetPidStatus(true); | ||
1197 | } | ||
1198 | else | ||
1199 | { | 863 | { |
864 | case (int)ActorTypes.Agent: | ||
865 | p1.CollidingObj = true; | ||
866 | p2.CollidingObj = true; | ||
867 | break; | ||
868 | |||
869 | case (int)ActorTypes.Prim: | ||
870 | if (p2.Velocity.LengthSquared() > 0.0f) | ||
871 | p2.CollidingObj = true; | ||
872 | dop1foot = true; | ||
873 | break; | ||
1200 | 874 | ||
1201 | //contact.depth = 0.0000000f; | 875 | default: |
876 | ignore = true; // avatar to terrain and water ignored | ||
877 | break; | ||
1202 | } | 878 | } |
1203 | 879 | break; | |
1204 | |||
1205 | |||
1206 | } | ||
1207 | */ | ||
1208 | // If you interpenetrate a prim with another prim | ||
1209 | /* | ||
1210 | if (p1.PhysicsActorType == (int) ActorTypes.Prim && p2.PhysicsActorType == (int) ActorTypes.Prim) | ||
1211 | { | ||
1212 | #region disabledcode2 | ||
1213 | //OdePrim op1 = (OdePrim)p1; | ||
1214 | //OdePrim op2 = (OdePrim)p2; | ||
1215 | //op1.m_collisionscore++; | ||
1216 | //op2.m_collisionscore++; | ||
1217 | |||
1218 | //if (op1.m_collisionscore > 8000 || op2.m_collisionscore > 8000) | ||
1219 | //{ | ||
1220 | //op1.m_taintdisable = true; | ||
1221 | //AddPhysicsActorTaint(p1); | ||
1222 | //op2.m_taintdisable = true; | ||
1223 | //AddPhysicsActorTaint(p2); | ||
1224 | //} | ||
1225 | |||
1226 | //if (contact.depth >= 0.25f) | ||
1227 | //{ | ||
1228 | // Don't collide, one or both prim will expld. | ||
1229 | |||
1230 | //op1.m_interpenetrationcount++; | ||
1231 | //op2.m_interpenetrationcount++; | ||
1232 | //interpenetrations_before_disable = 200; | ||
1233 | //if (op1.m_interpenetrationcount >= interpenetrations_before_disable) | ||
1234 | //{ | ||
1235 | //op1.m_taintdisable = true; | ||
1236 | //AddPhysicsActorTaint(p1); | ||
1237 | //} | ||
1238 | //if (op2.m_interpenetrationcount >= interpenetrations_before_disable) | ||
1239 | //{ | ||
1240 | // op2.m_taintdisable = true; | ||
1241 | //AddPhysicsActorTaint(p2); | ||
1242 | //} | ||
1243 | |||
1244 | //contact.depth = contact.depth / 8f; | ||
1245 | //contact.normal = new d.Vector3(0, 0, 1); | ||
1246 | //} | ||
1247 | //if (op1.m_disabled || op2.m_disabled) | ||
1248 | //{ | ||
1249 | //Manually disabled objects stay disabled | ||
1250 | //contact.depth = 0f; | ||
1251 | //} | ||
1252 | #endregion | ||
1253 | } | 880 | } |
1254 | */ | 881 | |
1255 | #endregion | 882 | case (int)ActorTypes.Prim: |
1256 | if (curContact.depth >= 1.00f) | 883 | switch (p2.PhysicsActorType) |
1257 | { | 884 | { |
1258 | //m_log.Info("[P]: " + contact.depth.ToString()); | 885 | case (int)ActorTypes.Agent: |
1259 | if ((p2.PhysicsActorType == (int) ActorTypes.Agent && | 886 | AvanormOverride = true; |
1260 | p1.PhysicsActorType == (int) ActorTypes.Unknown) || | 887 | |
1261 | (p1.PhysicsActorType == (int) ActorTypes.Agent && | 888 | Vector3 tmp = p2.Position - p1.Position; |
1262 | p2.PhysicsActorType == (int) ActorTypes.Unknown)) | 889 | normoverride = p2.Velocity - p1.Velocity; |
1263 | { | 890 | mu = normoverride.LengthSquared(); |
1264 | if (p2.PhysicsActorType == (int) ActorTypes.Agent) | 891 | if (mu > 1e-6) |
1265 | { | 892 | { |
1266 | if (p2 is OdeCharacter) | 893 | mu = 1.0f / (float)Math.Sqrt(mu); |
1267 | { | 894 | normoverride *= mu; |
1268 | OdeCharacter character = (OdeCharacter) p2; | 895 | mu = Vector3.Dot(tmp, normoverride); |
1269 | 896 | if (mu > 0) | |
1270 | //p2.CollidingObj = true; | 897 | normoverride *= -1; |
1271 | curContact.depth = 0.00000003f; | 898 | } |
1272 | p2.Velocity = p2.Velocity + new Vector3(0f, 0f, 0.5f); | 899 | else |
1273 | curContact.pos = | 900 | { |
1274 | new d.Vector3(curContact.pos.X + (p1.Size.X/2), | 901 | tmp.Normalize(); |
1275 | curContact.pos.Y + (p1.Size.Y/2), | 902 | normoverride = -tmp; |
1276 | curContact.pos.Z + (p1.Size.Z/2)); | 903 | } |
1277 | character.SetPidStatus(true); | 904 | |
1278 | } | 905 | bounce = 0; |
906 | mu = 0; | ||
907 | cfm = 0.0001f; | ||
908 | |||
909 | dop2foot = true; | ||
910 | if (p1.Velocity.LengthSquared() > 0.0f) | ||
911 | p1.CollidingObj = true; | ||
912 | break; | ||
913 | |||
914 | case (int)ActorTypes.Prim: | ||
915 | if ((p1.Velocity - p2.Velocity).LengthSquared() > 0.0f) | ||
916 | { | ||
917 | p1.CollidingObj = true; | ||
918 | p2.CollidingObj = true; | ||
1279 | } | 919 | } |
920 | p1.getContactData(ref contactdata1); | ||
921 | p2.getContactData(ref contactdata2); | ||
922 | bounce = contactdata1.bounce * contactdata2.bounce; | ||
923 | mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); | ||
924 | |||
925 | cfm = p1.Mass; | ||
926 | if (cfm > p2.Mass) | ||
927 | cfm = p2.Mass; | ||
928 | dscale = 10 / cfm; | ||
929 | dscale = (float)Math.Sqrt(dscale); | ||
930 | if (dscale > 1.0f) | ||
931 | dscale = 1.0f; | ||
932 | erpscale = cfm * 0.01f; | ||
933 | cfm = 0.0001f / cfm; | ||
934 | if (cfm > 0.01f) | ||
935 | cfm = 0.01f; | ||
936 | else if (cfm < 0.00001f) | ||
937 | cfm = 0.00001f; | ||
938 | |||
939 | if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) | ||
940 | mu *= frictionMovementMult; | ||
1280 | 941 | ||
1281 | if (p1.PhysicsActorType == (int) ActorTypes.Agent) | 942 | break; |
943 | |||
944 | case (int)ActorTypes.Ground: | ||
945 | p1.getContactData(ref contactdata1); | ||
946 | bounce = contactdata1.bounce * TerrainBounce; | ||
947 | mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction); | ||
948 | if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f) | ||
949 | mu *= frictionMovementMult; | ||
950 | p1.CollidingGround = true; | ||
951 | |||
952 | cfm = p1.Mass; | ||
953 | dscale = 10 / cfm; | ||
954 | dscale = (float)Math.Sqrt(dscale); | ||
955 | if (dscale > 1.0f) | ||
956 | dscale = 1.0f; | ||
957 | erpscale = cfm * 0.01f; | ||
958 | cfm = 0.0001f / cfm; | ||
959 | if (cfm > 0.01f) | ||
960 | cfm = 0.01f; | ||
961 | else if (cfm < 0.00001f) | ||
962 | cfm = 0.00001f; | ||
963 | |||
964 | if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass) | ||
1282 | { | 965 | { |
1283 | if (p1 is OdeCharacter) | 966 | if (curContact.side1 > 0) |
1284 | { | 967 | IgnoreNegSides = true; |
1285 | OdeCharacter character = (OdeCharacter) p1; | ||
1286 | |||
1287 | //p2.CollidingObj = true; | ||
1288 | curContact.depth = 0.00000003f; | ||
1289 | p1.Velocity = p1.Velocity + new Vector3(0f, 0f, 0.5f); | ||
1290 | curContact.pos = | ||
1291 | new d.Vector3(curContact.pos.X + (p1.Size.X/2), | ||
1292 | curContact.pos.Y + (p1.Size.Y/2), | ||
1293 | curContact.pos.Z + (p1.Size.Z/2)); | ||
1294 | character.SetPidStatus(true); | ||
1295 | } | ||
1296 | } | 968 | } |
1297 | } | 969 | break; |
970 | |||
971 | case (int)ActorTypes.Water: | ||
972 | default: | ||
973 | ignore = true; | ||
974 | break; | ||
1298 | } | 975 | } |
1299 | } | 976 | break; |
977 | |||
978 | case (int)ActorTypes.Ground: | ||
979 | if (p2.PhysicsActorType == (int)ActorTypes.Prim) | ||
980 | { | ||
981 | p2.CollidingGround = true; | ||
982 | p2.getContactData(ref contactdata2); | ||
983 | bounce = contactdata2.bounce * TerrainBounce; | ||
984 | mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction); | ||
985 | |||
986 | cfm = p2.Mass; | ||
987 | dscale = 10 / cfm; | ||
988 | dscale = (float)Math.Sqrt(dscale); | ||
989 | |||
990 | if (dscale > 1.0f) | ||
991 | dscale = 1.0f; | ||
1300 | 992 | ||
1301 | #endregion | 993 | erpscale = cfm * 0.01f; |
994 | cfm = 0.0001f / cfm; | ||
995 | if (cfm > 0.01f) | ||
996 | cfm = 0.01f; | ||
997 | else if (cfm < 0.00001f) | ||
998 | cfm = 0.00001f; | ||
1302 | 999 | ||
1303 | // Logic for collision handling | 1000 | if (curContact.side1 > 0) // should be 2 ? |
1304 | // Note, that if *all* contacts are skipped (VolumeDetect) | 1001 | IgnoreNegSides = true; |
1305 | // The prim still detects (and forwards) collision events but | ||
1306 | // appears to be phantom for the world | ||
1307 | Boolean skipThisContact = false; | ||
1308 | 1002 | ||
1309 | if ((p1 is OdePrim) && (((OdePrim)p1).m_isVolumeDetect)) | 1003 | if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f) |
1310 | skipThisContact = true; // No collision on volume detect prims | 1004 | mu *= frictionMovementMult; |
1005 | } | ||
1006 | else | ||
1007 | ignore = true; | ||
1008 | break; | ||
1009 | |||
1010 | case (int)ActorTypes.Water: | ||
1011 | default: | ||
1012 | break; | ||
1013 | } | ||
1014 | if (ignore) | ||
1015 | return; | ||
1311 | 1016 | ||
1312 | if (!skipThisContact && (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect)) | 1017 | IntPtr Joint; |
1313 | skipThisContact = true; // No collision on volume detect prims | ||
1314 | 1018 | ||
1315 | if (!skipThisContact && curContact.depth < 0f) | 1019 | int i = 0; |
1316 | skipThisContact = true; | 1020 | while(true) |
1021 | { | ||
1317 | 1022 | ||
1318 | if (!skipThisContact && checkDupe(curContact, p2.PhysicsActorType)) | 1023 | if (IgnoreNegSides && curContact.side1 < 0) |
1319 | skipThisContact = true; | 1024 | { |
1025 | if (++i >= count) | ||
1026 | break; | ||
1320 | 1027 | ||
1321 | const int maxContactsbeforedeath = 4000; | 1028 | if (!GetCurContactGeom(i, ref curContact)) |
1322 | joint = IntPtr.Zero; | 1029 | break; |
1030 | } | ||
1031 | else | ||
1323 | 1032 | ||
1324 | if (!skipThisContact) | ||
1325 | { | 1033 | { |
1326 | _perloopContact.Add(curContact); | ||
1327 | 1034 | ||
1328 | if (name1 == "Terrain" || name2 == "Terrain") | 1035 | if (AvanormOverride) |
1329 | { | 1036 | { |
1330 | if ((p2.PhysicsActorType == (int) ActorTypes.Agent) && | 1037 | if (curContact.depth > 0.3f) |
1331 | (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)) | ||
1332 | { | 1038 | { |
1333 | p2ExpectedPoints = avatarExpectedContacts; | 1039 | if (dop1foot && (p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f) |
1334 | // Avatar is moving on terrain, use the movement terrain contact | 1040 | p1.IsColliding = true; |
1335 | AvatarMovementTerrainContact.geom = curContact; | 1041 | if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f) |
1336 | 1042 | p2.IsColliding = true; | |
1337 | if (m_global_contactcount < maxContactsbeforedeath) | 1043 | curContact.normal.X = normoverride.X; |
1338 | { | 1044 | curContact.normal.Y = normoverride.Y; |
1339 | joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementTerrainContact); | 1045 | curContact.normal.Z = normoverride.Z; |
1340 | m_global_contactcount++; | ||
1341 | } | ||
1342 | } | 1046 | } |
1047 | |||
1343 | else | 1048 | else |
1344 | { | 1049 | { |
1345 | if (p2.PhysicsActorType == (int)ActorTypes.Agent) | 1050 | if (dop1foot) |
1346 | { | ||
1347 | p2ExpectedPoints = avatarExpectedContacts; | ||
1348 | // Avatar is standing on terrain, use the non moving terrain contact | ||
1349 | TerrainContact.geom = curContact; | ||
1350 | |||
1351 | if (m_global_contactcount < maxContactsbeforedeath) | ||
1352 | { | ||
1353 | joint = d.JointCreateContact(world, contactgroup, ref TerrainContact); | ||
1354 | m_global_contactcount++; | ||
1355 | } | ||
1356 | } | ||
1357 | else | ||
1358 | { | 1051 | { |
1359 | if (p2.PhysicsActorType == (int)ActorTypes.Prim && p1.PhysicsActorType == (int)ActorTypes.Prim) | 1052 | float sz = p1.Size.Z; |
1053 | Vector3 vtmp = p1.Position; | ||
1054 | float ppos = curContact.pos.Z - vtmp.Z + (sz - avCapRadius) * 0.5f; | ||
1055 | if (ppos > 0f) | ||
1360 | { | 1056 | { |
1361 | // prim prim contact | 1057 | if (!p1.Flying) |
1362 | // int pj294950 = 0; | ||
1363 | int movintYN = 0; | ||
1364 | int material = (int) Material.Wood; | ||
1365 | // prim terrain contact | ||
1366 | if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f) | ||
1367 | { | ||
1368 | movintYN = 1; | ||
1369 | } | ||
1370 | |||
1371 | if (p2 is OdePrim) | ||
1372 | { | 1058 | { |
1373 | material = ((OdePrim) p2).m_material; | 1059 | d.AABB aabb; |
1374 | p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts; | 1060 | d.GeomGetAABB(g2, out aabb); |
1375 | } | 1061 | float tmp = vtmp.Z - sz * .18f; |
1376 | |||
1377 | // Unnessesary because p1 is defined above | ||
1378 | //if (p1 is OdePrim) | ||
1379 | // { | ||
1380 | // p1ExpectedPoints = ((OdePrim)p1).ExpectedCollisionContacts; | ||
1381 | // } | ||
1382 | //m_log.DebugFormat("Material: {0}", material); | ||
1383 | |||
1384 | m_materialContacts[material, movintYN].geom = curContact; | ||
1385 | 1062 | ||
1386 | if (m_global_contactcount < maxContactsbeforedeath) | 1063 | if (aabb.MaxZ < tmp) |
1387 | { | 1064 | { |
1388 | joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]); | 1065 | vtmp.X = curContact.pos.X - vtmp.X; |
1389 | m_global_contactcount++; | 1066 | vtmp.Y = curContact.pos.Y - vtmp.Y; |
1067 | vtmp.Z = -0.2f; | ||
1068 | vtmp.Normalize(); | ||
1069 | curContact.normal.X = vtmp.X; | ||
1070 | curContact.normal.Y = vtmp.Y; | ||
1071 | curContact.normal.Z = vtmp.Z; | ||
1072 | } | ||
1390 | } | 1073 | } |
1391 | } | 1074 | } |
1392 | else | 1075 | else |
1393 | { | 1076 | p1.IsColliding = true; |
1394 | int movintYN = 0; | ||
1395 | // prim terrain contact | ||
1396 | if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f) | ||
1397 | { | ||
1398 | movintYN = 1; | ||
1399 | } | ||
1400 | 1077 | ||
1401 | int material = (int)Material.Wood; | ||
1402 | |||
1403 | if (p2 is OdePrim) | ||
1404 | { | ||
1405 | material = ((OdePrim)p2).m_material; | ||
1406 | p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts; | ||
1407 | } | ||
1408 | |||
1409 | //m_log.DebugFormat("Material: {0}", material); | ||
1410 | m_materialContacts[material, movintYN].geom = curContact; | ||
1411 | |||
1412 | if (m_global_contactcount < maxContactsbeforedeath) | ||
1413 | { | ||
1414 | joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]); | ||
1415 | m_global_contactcount++; | ||
1416 | } | ||
1417 | } | ||
1418 | } | 1078 | } |
1419 | } | ||
1420 | //if (p2.PhysicsActorType == (int)ActorTypes.Prim) | ||
1421 | //{ | ||
1422 | //m_log.Debug("[PHYSICS]: prim contacting with ground"); | ||
1423 | //} | ||
1424 | } | ||
1425 | else if (name1 == "Water" || name2 == "Water") | ||
1426 | { | ||
1427 | /* | ||
1428 | if ((p2.PhysicsActorType == (int) ActorTypes.Prim)) | ||
1429 | { | ||
1430 | } | ||
1431 | else | ||
1432 | { | ||
1433 | } | ||
1434 | */ | ||
1435 | //WaterContact.surface.soft_cfm = 0.0000f; | ||
1436 | //WaterContact.surface.soft_erp = 0.00000f; | ||
1437 | if (curContact.depth > 0.1f) | ||
1438 | { | ||
1439 | curContact.depth *= 52; | ||
1440 | //contact.normal = new d.Vector3(0, 0, 1); | ||
1441 | //contact.pos = new d.Vector3(0, 0, contact.pos.Z - 5f); | ||
1442 | } | ||
1443 | |||
1444 | WaterContact.geom = curContact; | ||
1445 | 1079 | ||
1446 | if (m_global_contactcount < maxContactsbeforedeath) | 1080 | if (dop2foot) |
1447 | { | ||
1448 | joint = d.JointCreateContact(world, contactgroup, ref WaterContact); | ||
1449 | m_global_contactcount++; | ||
1450 | } | ||
1451 | //m_log.Info("[PHYSICS]: Prim Water Contact" + contact.depth); | ||
1452 | } | ||
1453 | else | ||
1454 | { | ||
1455 | if ((p2.PhysicsActorType == (int)ActorTypes.Agent)) | ||
1456 | { | ||
1457 | p2ExpectedPoints = avatarExpectedContacts; | ||
1458 | if ((Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)) | ||
1459 | { | 1081 | { |
1460 | // Avatar is moving on a prim, use the Movement prim contact | 1082 | float sz = p2.Size.Z; |
1461 | AvatarMovementprimContact.geom = curContact; | 1083 | Vector3 vtmp = p2.Position; |
1462 | 1084 | float ppos = curContact.pos.Z - vtmp.Z + (sz - avCapRadius) * 0.5f; | |
1463 | if (m_global_contactcount < maxContactsbeforedeath) | 1085 | if (ppos > 0f) |
1464 | { | 1086 | { |
1465 | joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementprimContact); | 1087 | if (!p2.Flying) |
1466 | m_global_contactcount++; | 1088 | { |
1467 | } | 1089 | d.AABB aabb; |
1468 | } | 1090 | d.GeomGetAABB(g1, out aabb); |
1469 | else | 1091 | float tmp = vtmp.Z - sz * .18f; |
1470 | { | ||
1471 | // Avatar is standing still on a prim, use the non movement contact | ||
1472 | contact.geom = curContact; | ||
1473 | 1092 | ||
1474 | if (m_global_contactcount < maxContactsbeforedeath) | 1093 | if (aabb.MaxZ < tmp) |
1475 | { | 1094 | { |
1476 | joint = d.JointCreateContact(world, contactgroup, ref contact); | 1095 | vtmp.X = curContact.pos.X - vtmp.X; |
1477 | m_global_contactcount++; | 1096 | vtmp.Y = curContact.pos.Y - vtmp.Y; |
1097 | vtmp.Z = -0.2f; | ||
1098 | vtmp.Normalize(); | ||
1099 | curContact.normal.X = vtmp.X; | ||
1100 | curContact.normal.Y = vtmp.Y; | ||
1101 | curContact.normal.Z = vtmp.Z; | ||
1102 | } | ||
1103 | } | ||
1478 | } | 1104 | } |
1479 | } | 1105 | else |
1480 | } | 1106 | p2.IsColliding = true; |
1481 | else if (p2.PhysicsActorType == (int)ActorTypes.Prim) | ||
1482 | { | ||
1483 | //p1.PhysicsActorType | ||
1484 | int material = (int)Material.Wood; | ||
1485 | |||
1486 | if (p2 is OdePrim) | ||
1487 | { | ||
1488 | material = ((OdePrim)p2).m_material; | ||
1489 | p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts; | ||
1490 | } | ||
1491 | |||
1492 | //m_log.DebugFormat("Material: {0}", material); | ||
1493 | m_materialContacts[material, 0].geom = curContact; | ||
1494 | 1107 | ||
1495 | if (m_global_contactcount < maxContactsbeforedeath) | ||
1496 | { | ||
1497 | joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, 0]); | ||
1498 | m_global_contactcount++; | ||
1499 | } | 1108 | } |
1500 | } | 1109 | } |
1501 | } | 1110 | } |
1502 | 1111 | ||
1503 | if (m_global_contactcount < maxContactsbeforedeath && joint != IntPtr.Zero) // stack collide! | 1112 | Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale); |
1504 | { | 1113 | d.JointAttach(Joint, b1, b2); |
1505 | d.JointAttach(joint, b1, b2); | ||
1506 | m_global_contactcount++; | ||
1507 | } | ||
1508 | } | ||
1509 | |||
1510 | collision_accounting_events(p1, p2, maxDepthContact); | ||
1511 | |||
1512 | if (count > ((p1ExpectedPoints + p2ExpectedPoints) * 0.25) + (geomContactPointsStartthrottle)) | ||
1513 | { | ||
1514 | // If there are more then 3 contact points, it's likely | ||
1515 | // that we've got a pile of objects, so ... | ||
1516 | // We don't want to send out hundreds of terse updates over and over again | ||
1517 | // so lets throttle them and send them again after it's somewhat sorted out. | ||
1518 | p2.ThrottleUpdates = true; | ||
1519 | } | ||
1520 | //m_log.Debug(count.ToString()); | ||
1521 | //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2); | ||
1522 | } | ||
1523 | } | ||
1524 | 1114 | ||
1525 | private bool checkDupe(d.ContactGeom contactGeom, int atype) | 1115 | if (++m_global_contactcount >= maxContactsbeforedeath) |
1526 | { | 1116 | break; |
1527 | if (!m_filterCollisions) | ||
1528 | return false; | ||
1529 | |||
1530 | bool result = false; | ||
1531 | 1117 | ||
1532 | ActorTypes at = (ActorTypes)atype; | 1118 | if (++i >= count) |
1119 | break; | ||
1533 | 1120 | ||
1534 | foreach (d.ContactGeom contact in _perloopContact) | 1121 | if (!GetCurContactGeom(i, ref curContact)) |
1535 | { | 1122 | break; |
1536 | //if ((contact.g1 == contactGeom.g1 && contact.g2 == contactGeom.g2)) | ||
1537 | //{ | ||
1538 | // || (contact.g2 == contactGeom.g1 && contact.g1 == contactGeom.g2) | ||
1539 | if (at == ActorTypes.Agent) | ||
1540 | { | ||
1541 | if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f) | ||
1542 | && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f) | ||
1543 | && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f))) | ||
1544 | { | ||
1545 | if (Math.Abs(contact.depth - contactGeom.depth) < 0.052f) | ||
1546 | { | ||
1547 | //contactGeom.depth *= .00005f; | ||
1548 | //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth)); | ||
1549 | // m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z)); | ||
1550 | result = true; | ||
1551 | break; | ||
1552 | } | ||
1553 | // else | ||
1554 | // { | ||
1555 | // //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth)); | ||
1556 | // } | ||
1557 | } | ||
1558 | // else | ||
1559 | // { | ||
1560 | // //m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z)); | ||
1561 | // //int i = 0; | ||
1562 | // } | ||
1563 | } | ||
1564 | else if (at == ActorTypes.Prim) | ||
1565 | { | ||
1566 | //d.AABB aabb1 = new d.AABB(); | ||
1567 | //d.AABB aabb2 = new d.AABB(); | ||
1568 | 1123 | ||
1569 | //d.GeomGetAABB(contactGeom.g2, out aabb2); | 1124 | if (curContact.depth > maxDepthContact.PenetrationDepth) |
1570 | //d.GeomGetAABB(contactGeom.g1, out aabb1); | ||
1571 | //aabb1. | ||
1572 | if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f) && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f) && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f))) | ||
1573 | { | 1125 | { |
1574 | if (contactGeom.normal.X == contact.normal.X && contactGeom.normal.Y == contact.normal.Y && contactGeom.normal.Z == contact.normal.Z) | 1126 | maxDepthContact.Position.X = curContact.pos.X; |
1575 | { | 1127 | maxDepthContact.Position.Y = curContact.pos.Y; |
1576 | if (Math.Abs(contact.depth - contactGeom.depth) < 0.272f) | 1128 | maxDepthContact.Position.Z = curContact.pos.Z; |
1577 | { | 1129 | maxDepthContact.SurfaceNormal.X = curContact.normal.X; |
1578 | result = true; | 1130 | maxDepthContact.SurfaceNormal.Y = curContact.normal.Y; |
1579 | break; | 1131 | maxDepthContact.SurfaceNormal.Z = curContact.normal.Z; |
1580 | } | 1132 | maxDepthContact.PenetrationDepth = curContact.depth; |
1581 | } | ||
1582 | //m_log.DebugFormat("[Collision]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth)); | ||
1583 | //m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z)); | ||
1584 | } | 1133 | } |
1585 | } | 1134 | } |
1586 | } | 1135 | } |
1587 | 1136 | ||
1588 | return result; | 1137 | collision_accounting_events(p1, p2, maxDepthContact); |
1138 | |||
1139 | /* | ||
1140 | if (notskipedcount > geomContactPointsStartthrottle) | ||
1141 | { | ||
1142 | // If there are more then 3 contact points, it's likely | ||
1143 | // that we've got a pile of objects, so ... | ||
1144 | // We don't want to send out hundreds of terse updates over and over again | ||
1145 | // so lets throttle them and send them again after it's somewhat sorted out. | ||
1146 | this needs checking so out for now | ||
1147 | if (b1 != IntPtr.Zero) | ||
1148 | p1.ThrottleUpdates = true; | ||
1149 | if (b2 != IntPtr.Zero) | ||
1150 | p2.ThrottleUpdates = true; | ||
1151 | |||
1152 | } | ||
1153 | */ | ||
1589 | } | 1154 | } |
1590 | 1155 | ||
1591 | private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact) | 1156 | private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact) |
1592 | { | 1157 | { |
1593 | // obj1LocalID = 0; | 1158 | uint obj2LocalID = 0; |
1594 | //returncollisions = false; | ||
1595 | obj2LocalID = 0; | ||
1596 | //ctype = 0; | ||
1597 | //cStartStop = 0; | ||
1598 | if (!p2.SubscribedEvents() && !p1.SubscribedEvents()) | ||
1599 | return; | ||
1600 | 1159 | ||
1601 | switch ((ActorTypes)p2.PhysicsActorType) | 1160 | bool p1events = p1.SubscribedEvents(); |
1602 | { | 1161 | bool p2events = p2.SubscribedEvents(); |
1603 | case ActorTypes.Agent: | ||
1604 | cc2 = (OdeCharacter)p2; | ||
1605 | 1162 | ||
1606 | // obj1LocalID = cc2.m_localID; | 1163 | if (p1.IsVolumeDtc) |
1607 | switch ((ActorTypes)p1.PhysicsActorType) | 1164 | p2events = false; |
1608 | { | 1165 | if (p2.IsVolumeDtc) |
1609 | case ActorTypes.Agent: | 1166 | p1events = false; |
1610 | cc1 = (OdeCharacter)p1; | ||
1611 | obj2LocalID = cc1.LocalID; | ||
1612 | cc1.AddCollisionEvent(cc2.LocalID, contact); | ||
1613 | //ctype = (int)CollisionCategories.Character; | ||
1614 | |||
1615 | //if (cc1.CollidingObj) | ||
1616 | //cStartStop = (int)StatusIndicators.Generic; | ||
1617 | //else | ||
1618 | //cStartStop = (int)StatusIndicators.Start; | ||
1619 | |||
1620 | //returncollisions = true; | ||
1621 | break; | ||
1622 | 1167 | ||
1623 | case ActorTypes.Prim: | 1168 | if (!p2events && !p1events) |
1624 | if (p1 is OdePrim) | 1169 | return; |
1625 | { | ||
1626 | cp1 = (OdePrim) p1; | ||
1627 | obj2LocalID = cp1.LocalID; | ||
1628 | cp1.AddCollisionEvent(cc2.LocalID, contact); | ||
1629 | } | ||
1630 | //ctype = (int)CollisionCategories.Geom; | ||
1631 | 1170 | ||
1632 | //if (cp1.CollidingObj) | 1171 | Vector3 vel = Vector3.Zero; |
1633 | //cStartStop = (int)StatusIndicators.Generic; | 1172 | if (p2 != null && p2.IsPhysical) |
1634 | //else | 1173 | vel = p2.Velocity; |
1635 | //cStartStop = (int)StatusIndicators.Start; | ||
1636 | 1174 | ||
1637 | //returncollisions = true; | 1175 | if (p1 != null && p1.IsPhysical) |
1638 | break; | 1176 | vel -= p1.Velocity; |
1639 | 1177 | ||
1640 | case ActorTypes.Ground: | 1178 | contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal); |
1641 | case ActorTypes.Unknown: | ||
1642 | obj2LocalID = 0; | ||
1643 | //ctype = (int)CollisionCategories.Land; | ||
1644 | //returncollisions = true; | ||
1645 | break; | ||
1646 | } | ||
1647 | |||
1648 | cc2.AddCollisionEvent(obj2LocalID, contact); | ||
1649 | break; | ||
1650 | 1179 | ||
1180 | switch ((ActorTypes)p1.PhysicsActorType) | ||
1181 | { | ||
1182 | case ActorTypes.Agent: | ||
1651 | case ActorTypes.Prim: | 1183 | case ActorTypes.Prim: |
1652 | |||
1653 | if (p2 is OdePrim) | ||
1654 | { | 1184 | { |
1655 | cp2 = (OdePrim) p2; | 1185 | switch ((ActorTypes)p2.PhysicsActorType) |
1656 | |||
1657 | // obj1LocalID = cp2.m_localID; | ||
1658 | switch ((ActorTypes) p1.PhysicsActorType) | ||
1659 | { | 1186 | { |
1660 | case ActorTypes.Agent: | 1187 | case ActorTypes.Agent: |
1661 | if (p1 is OdeCharacter) | ||
1662 | { | ||
1663 | cc1 = (OdeCharacter) p1; | ||
1664 | obj2LocalID = cc1.LocalID; | ||
1665 | cc1.AddCollisionEvent(cp2.LocalID, contact); | ||
1666 | //ctype = (int)CollisionCategories.Character; | ||
1667 | |||
1668 | //if (cc1.CollidingObj) | ||
1669 | //cStartStop = (int)StatusIndicators.Generic; | ||
1670 | //else | ||
1671 | //cStartStop = (int)StatusIndicators.Start; | ||
1672 | //returncollisions = true; | ||
1673 | } | ||
1674 | break; | ||
1675 | case ActorTypes.Prim: | 1188 | case ActorTypes.Prim: |
1676 | 1189 | if (p2events) | |
1677 | if (p1 is OdePrim) | ||
1678 | { | 1190 | { |
1679 | cp1 = (OdePrim) p1; | 1191 | AddCollisionEventReporting(p2); |
1680 | obj2LocalID = cp1.LocalID; | 1192 | p2.AddCollisionEvent(p1.ParentActor.LocalID, contact); |
1681 | cp1.AddCollisionEvent(cp2.LocalID, contact); | ||
1682 | //ctype = (int)CollisionCategories.Geom; | ||
1683 | |||
1684 | //if (cp1.CollidingObj) | ||
1685 | //cStartStop = (int)StatusIndicators.Generic; | ||
1686 | //else | ||
1687 | //cStartStop = (int)StatusIndicators.Start; | ||
1688 | |||
1689 | //returncollisions = true; | ||
1690 | } | 1193 | } |
1194 | obj2LocalID = p2.ParentActor.LocalID; | ||
1691 | break; | 1195 | break; |
1692 | 1196 | ||
1693 | case ActorTypes.Ground: | 1197 | case ActorTypes.Ground: |
1694 | case ActorTypes.Unknown: | 1198 | case ActorTypes.Unknown: |
1199 | default: | ||
1695 | obj2LocalID = 0; | 1200 | obj2LocalID = 0; |
1696 | //ctype = (int)CollisionCategories.Land; | ||
1697 | |||
1698 | //returncollisions = true; | ||
1699 | break; | 1201 | break; |
1700 | } | 1202 | } |
1701 | 1203 | if (p1events) | |
1702 | cp2.AddCollisionEvent(obj2LocalID, contact); | ||
1703 | } | ||
1704 | break; | ||
1705 | } | ||
1706 | //if (returncollisions) | ||
1707 | //{ | ||
1708 | |||
1709 | //lock (m_storedCollisions) | ||
1710 | //{ | ||
1711 | //cDictKey = obj1LocalID.ToString() + obj2LocalID.ToString() + cStartStop.ToString() + ctype.ToString(); | ||
1712 | //if (m_storedCollisions.ContainsKey(cDictKey)) | ||
1713 | //{ | ||
1714 | //sCollisionData objd = m_storedCollisions[cDictKey]; | ||
1715 | //objd.NumberOfCollisions += 1; | ||
1716 | //objd.lastframe = framecount; | ||
1717 | //m_storedCollisions[cDictKey] = objd; | ||
1718 | //} | ||
1719 | //else | ||
1720 | //{ | ||
1721 | //sCollisionData objd = new sCollisionData(); | ||
1722 | //objd.ColliderLocalId = obj1LocalID; | ||
1723 | //objd.CollidedWithLocalId = obj2LocalID; | ||
1724 | //objd.CollisionType = ctype; | ||
1725 | //objd.NumberOfCollisions = 1; | ||
1726 | //objd.lastframe = framecount; | ||
1727 | //objd.StatusIndicator = cStartStop; | ||
1728 | //m_storedCollisions.Add(cDictKey, objd); | ||
1729 | //} | ||
1730 | //} | ||
1731 | // } | ||
1732 | } | ||
1733 | |||
1734 | private int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount) | ||
1735 | { | ||
1736 | /* String name1 = null; | ||
1737 | String name2 = null; | ||
1738 | |||
1739 | if (!geom_name_map.TryGetValue(trimesh, out name1)) | ||
1740 | { | 1204 | { |
1741 | name1 = "null"; | 1205 | contact.SurfaceNormal = -contact.SurfaceNormal; |
1206 | AddCollisionEventReporting(p1); | ||
1207 | p1.AddCollisionEvent(obj2LocalID, contact); | ||
1742 | } | 1208 | } |
1743 | if (!geom_name_map.TryGetValue(refObject, out name2)) | 1209 | break; |
1210 | } | ||
1211 | case ActorTypes.Ground: | ||
1212 | case ActorTypes.Unknown: | ||
1213 | default: | ||
1214 | { | ||
1215 | if (p2events && !p2.IsVolumeDtc) | ||
1744 | { | 1216 | { |
1745 | name2 = "null"; | 1217 | AddCollisionEventReporting(p2); |
1218 | p2.AddCollisionEvent(0, contact); | ||
1746 | } | 1219 | } |
1747 | 1220 | break; | |
1748 | m_log.InfoFormat("TriArrayCallback: A collision was detected between {1} and {2}", 0, name1, name2); | 1221 | } |
1749 | */ | 1222 | } |
1750 | return 1; | ||
1751 | } | ||
1752 | |||
1753 | private int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex) | ||
1754 | { | ||
1755 | // String name1 = null; | ||
1756 | // String name2 = null; | ||
1757 | // | ||
1758 | // if (!geom_name_map.TryGetValue(trimesh, out name1)) | ||
1759 | // { | ||
1760 | // name1 = "null"; | ||
1761 | // } | ||
1762 | // | ||
1763 | // if (!geom_name_map.TryGetValue(refObject, out name2)) | ||
1764 | // { | ||
1765 | // name2 = "null"; | ||
1766 | // } | ||
1767 | |||
1768 | // m_log.InfoFormat("TriCallback: A collision was detected between {1} and {2}. Index was {3}", 0, name1, name2, triangleIndex); | ||
1769 | |||
1770 | d.Vector3 v0 = new d.Vector3(); | ||
1771 | d.Vector3 v1 = new d.Vector3(); | ||
1772 | d.Vector3 v2 = new d.Vector3(); | ||
1773 | |||
1774 | d.GeomTriMeshGetTriangle(trimesh, 0, ref v0, ref v1, ref v2); | ||
1775 | // m_log.DebugFormat("Triangle {0} is <{1},{2},{3}>, <{4},{5},{6}>, <{7},{8},{9}>", triangleIndex, v0.X, v0.Y, v0.Z, v1.X, v1.Y, v1.Z, v2.X, v2.Y, v2.Z); | ||
1776 | |||
1777 | return 1; | ||
1778 | } | 1223 | } |
1779 | 1224 | ||
1780 | /// <summary> | 1225 | /// <summary> |
1781 | /// This is our collision testing routine in ODE | 1226 | /// This is our collision testing routine in ODE |
1782 | /// </summary> | 1227 | /// </summary> |
1228 | /// <param name="timeStep"></param> | ||
1783 | private void collision_optimized() | 1229 | private void collision_optimized() |
1784 | { | 1230 | { |
1785 | _perloopContact.Clear(); | 1231 | lock (_characters) |
1786 | 1232 | { | |
1787 | foreach (OdeCharacter chr in _characters) | ||
1788 | { | ||
1789 | // Reset the collision values to false | ||
1790 | // since we don't know if we're colliding yet | ||
1791 | if (chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero) | ||
1792 | continue; | ||
1793 | |||
1794 | chr.IsColliding = false; | ||
1795 | chr.CollidingGround = false; | ||
1796 | chr.CollidingObj = false; | ||
1797 | |||
1798 | // Test the avatar's geometry for collision with the space | ||
1799 | // This will return near and the space that they are the closest to | ||
1800 | // And we'll run this again against the avatar and the space segment | ||
1801 | // This will return with a bunch of possible objects in the space segment | ||
1802 | // and we'll run it again on all of them. | ||
1803 | try | 1233 | try |
1804 | { | 1234 | { |
1805 | CollideSpaces(space, chr.Shell, IntPtr.Zero); | 1235 | foreach (OdeCharacter chr in _characters) |
1236 | { | ||
1237 | if (chr == null || chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero) | ||
1238 | continue; | ||
1239 | |||
1240 | chr.IsColliding = false; | ||
1241 | // chr.CollidingGround = false; not done here | ||
1242 | chr.CollidingObj = false; | ||
1243 | // do colisions with static space | ||
1244 | d.SpaceCollide2(StaticSpace, chr.Shell, IntPtr.Zero, nearCallback); | ||
1245 | // no coll with gnd | ||
1246 | } | ||
1806 | } | 1247 | } |
1807 | catch (AccessViolationException) | 1248 | catch (AccessViolationException) |
1808 | { | 1249 | { |
1809 | m_log.ErrorFormat("[ODE SCENE]: Unable to space collide {0}", Name); | 1250 | m_log.Warn("[PHYSICS]: Unable to collide Character to static space"); |
1810 | } | 1251 | } |
1811 | 1252 | ||
1812 | //float terrainheight = GetTerrainHeightAtXY(chr.Position.X, chr.Position.Y); | ||
1813 | //if (chr.Position.Z + (chr.Velocity.Z * timeStep) < terrainheight + 10) | ||
1814 | //{ | ||
1815 | //chr.Position.Z = terrainheight + 10.0f; | ||
1816 | //forcedZ = true; | ||
1817 | //} | ||
1818 | } | 1253 | } |
1819 | 1254 | ||
1820 | if (CollectStats) | 1255 | lock (_activeprims) |
1821 | { | 1256 | { |
1822 | m_tempAvatarCollisionsThisFrame = _perloopContact.Count; | 1257 | foreach (OdePrim aprim in _activeprims) |
1823 | m_stats[ODEAvatarContactsStatsName] += m_tempAvatarCollisionsThisFrame; | 1258 | { |
1259 | aprim.CollisionScore = 0; | ||
1260 | aprim.IsColliding = false; | ||
1261 | } | ||
1824 | } | 1262 | } |
1825 | 1263 | ||
1826 | List<OdePrim> removeprims = null; | 1264 | // collide active prims with static enviroment |
1827 | foreach (OdePrim chr in _activeprims) | 1265 | lock (_activegroups) |
1828 | { | 1266 | { |
1829 | if (chr.Body != IntPtr.Zero && d.BodyIsEnabled(chr.Body) && (!chr.m_disabled)) | 1267 | try |
1830 | { | 1268 | { |
1831 | try | 1269 | foreach (OdePrim prm in _activegroups) |
1832 | { | 1270 | { |
1833 | lock (chr) | 1271 | if (!prm.m_outbounds) |
1834 | { | 1272 | { |
1835 | if (space != IntPtr.Zero && chr.prim_geom != IntPtr.Zero && chr.m_taintremove == false) | 1273 | if (d.BodyIsEnabled(prm.Body)) |
1836 | { | ||
1837 | CollideSpaces(space, chr.prim_geom, IntPtr.Zero); | ||
1838 | } | ||
1839 | else | ||
1840 | { | 1274 | { |
1841 | if (removeprims == null) | 1275 | d.SpaceCollide2(StaticSpace, prm.collide_geom, IntPtr.Zero, nearCallback); |
1842 | { | 1276 | d.SpaceCollide2(GroundSpace, prm.collide_geom, IntPtr.Zero, nearCallback); |
1843 | removeprims = new List<OdePrim>(); | ||
1844 | } | ||
1845 | removeprims.Add(chr); | ||
1846 | m_log.Error( | ||
1847 | "[ODE SCENE]: unable to collide test active prim against space. The space was zero, the geom was zero or it was in the process of being removed. Removed it from the active prim list. This needs to be fixed!"); | ||
1848 | } | 1277 | } |
1849 | } | 1278 | } |
1850 | } | 1279 | } |
1851 | catch (AccessViolationException) | ||
1852 | { | ||
1853 | m_log.Error("[ODE SCENE]: Unable to space collide"); | ||
1854 | } | ||
1855 | } | 1280 | } |
1856 | } | 1281 | catch (AccessViolationException) |
1857 | |||
1858 | if (CollectStats) | ||
1859 | m_stats[ODEPrimContactsStatName] += _perloopContact.Count - m_tempAvatarCollisionsThisFrame; | ||
1860 | |||
1861 | if (removeprims != null) | ||
1862 | { | ||
1863 | foreach (OdePrim chr in removeprims) | ||
1864 | { | 1282 | { |
1865 | _activeprims.Remove(chr); | 1283 | m_log.Warn("[PHYSICS]: Unable to collide Active prim to static space"); |
1866 | } | 1284 | } |
1867 | } | 1285 | } |
1868 | } | 1286 | // finally colide active things amoung them |
1869 | 1287 | try | |
1870 | #endregion | ||
1871 | |||
1872 | public override void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents) | ||
1873 | { | ||
1874 | m_worldOffset = offset; | ||
1875 | WorldExtents = new Vector2(extents.X, extents.Y); | ||
1876 | m_parentScene = pScene; | ||
1877 | } | ||
1878 | |||
1879 | // Recovered for use by fly height. Kitto Flora | ||
1880 | internal float GetTerrainHeightAtXY(float x, float y) | ||
1881 | { | ||
1882 | int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; | ||
1883 | int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; | ||
1884 | |||
1885 | IntPtr heightFieldGeom = IntPtr.Zero; | ||
1886 | |||
1887 | if (RegionTerrain.TryGetValue(new Vector3(offsetX,offsetY,0), out heightFieldGeom)) | ||
1888 | { | 1288 | { |
1889 | if (heightFieldGeom != IntPtr.Zero) | 1289 | d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback); |
1890 | { | ||
1891 | if (TerrainHeightFieldHeights.ContainsKey(heightFieldGeom)) | ||
1892 | { | ||
1893 | |||
1894 | int index; | ||
1895 | |||
1896 | |||
1897 | if ((int)x > WorldExtents.X || (int)y > WorldExtents.Y || | ||
1898 | (int)x < 0.001f || (int)y < 0.001f) | ||
1899 | return 0; | ||
1900 | |||
1901 | x = x - offsetX; | ||
1902 | y = y - offsetY; | ||
1903 | |||
1904 | index = (int)((int)x * ((int)Constants.RegionSize + 2) + (int)y); | ||
1905 | |||
1906 | if (index < TerrainHeightFieldHeights[heightFieldGeom].Length) | ||
1907 | { | ||
1908 | //m_log.DebugFormat("x{0} y{1} = {2}", x, y, (float)TerrainHeightFieldHeights[heightFieldGeom][index]); | ||
1909 | return (float)TerrainHeightFieldHeights[heightFieldGeom][index]; | ||
1910 | } | ||
1911 | |||
1912 | else | ||
1913 | return 0f; | ||
1914 | } | ||
1915 | else | ||
1916 | { | ||
1917 | return 0f; | ||
1918 | } | ||
1919 | |||
1920 | } | ||
1921 | else | ||
1922 | { | ||
1923 | return 0f; | ||
1924 | } | ||
1925 | |||
1926 | } | 1290 | } |
1927 | else | 1291 | catch (AccessViolationException) |
1928 | { | 1292 | { |
1929 | return 0f; | 1293 | m_log.Warn("[PHYSICS]: Unable to collide in Active space"); |
1930 | } | 1294 | } |
1931 | } | 1295 | // _perloopContact.Clear(); |
1932 | // End recovered. Kitto Flora | 1296 | } |
1933 | 1297 | ||
1298 | #endregion | ||
1934 | /// <summary> | 1299 | /// <summary> |
1935 | /// Add actor to the list that should receive collision events in the simulate loop. | 1300 | /// Add actor to the list that should receive collision events in the simulate loop. |
1936 | /// </summary> | 1301 | /// </summary> |
1937 | /// <param name="obj"></param> | 1302 | /// <param name="obj"></param> |
1938 | internal void AddCollisionEventReporting(PhysicsActor obj) | 1303 | public void AddCollisionEventReporting(PhysicsActor obj) |
1939 | { | 1304 | { |
1940 | // m_log.DebugFormat("[PHYSICS]: Adding {0} {1} to collision event reporting", obj.SOPName, obj.LocalID); | 1305 | if (!_collisionEventPrim.Contains(obj)) |
1941 | 1306 | _collisionEventPrim.Add(obj); | |
1942 | lock (m_collisionEventActorsChanges) | ||
1943 | m_collisionEventActorsChanges[obj.LocalID] = obj; | ||
1944 | } | 1307 | } |
1945 | 1308 | ||
1946 | /// <summary> | 1309 | /// <summary> |
1947 | /// Remove actor from the list that should receive collision events in the simulate loop. | 1310 | /// Remove actor from the list that should receive collision events in the simulate loop. |
1948 | /// </summary> | 1311 | /// </summary> |
1949 | /// <param name="obj"></param> | 1312 | /// <param name="obj"></param> |
1950 | internal void RemoveCollisionEventReporting(PhysicsActor obj) | 1313 | public void RemoveCollisionEventReporting(PhysicsActor obj) |
1951 | { | 1314 | { |
1952 | // m_log.DebugFormat("[PHYSICS]: Removing {0} {1} from collision event reporting", obj.SOPName, obj.LocalID); | 1315 | if (_collisionEventPrim.Contains(obj) && !_collisionEventPrimRemove.Contains(obj)) |
1316 | _collisionEventPrimRemove.Add(obj); | ||
1317 | } | ||
1953 | 1318 | ||
1954 | lock (m_collisionEventActorsChanges) | 1319 | public override float TimeDilation |
1955 | m_collisionEventActorsChanges[obj.LocalID] = null; | 1320 | { |
1321 | get { return m_timeDilation; } | ||
1322 | } | ||
1323 | |||
1324 | public override bool SupportsNINJAJoints | ||
1325 | { | ||
1326 | get { return false; } | ||
1956 | } | 1327 | } |
1957 | 1328 | ||
1958 | #region Add/Remove Entities | 1329 | #region Add/Remove Entities |
@@ -1963,470 +1334,118 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1963 | pos.X = position.X; | 1334 | pos.X = position.X; |
1964 | pos.Y = position.Y; | 1335 | pos.Y = position.Y; |
1965 | pos.Z = position.Z; | 1336 | pos.Z = position.Z; |
1966 | 1337 | OdeCharacter newAv = new OdeCharacter(avName, this, pos, size, avPIDD, avPIDP, avCapRadius, avDensity, avMovementDivisorWalk, avMovementDivisorRun); | |
1967 | OdeCharacter newAv | ||
1968 | = new OdeCharacter( | ||
1969 | avName, this, pos, size, avPIDD, avPIDP, | ||
1970 | avCapRadius, avStandupTensor, avDensity, | ||
1971 | avMovementDivisorWalk, avMovementDivisorRun); | ||
1972 | |||
1973 | newAv.Flying = isFlying; | 1338 | newAv.Flying = isFlying; |
1974 | newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset; | 1339 | newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset; |
1975 | 1340 | ||
1976 | return newAv; | 1341 | return newAv; |
1977 | } | 1342 | } |
1978 | 1343 | ||
1979 | public override void RemoveAvatar(PhysicsActor actor) | 1344 | public void AddCharacter(OdeCharacter chr) |
1980 | { | ||
1981 | // m_log.DebugFormat( | ||
1982 | // "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2}", | ||
1983 | // actor.Name, actor.LocalID, Name); | ||
1984 | |||
1985 | ((OdeCharacter) actor).Destroy(); | ||
1986 | } | ||
1987 | |||
1988 | internal void AddCharacter(OdeCharacter chr) | ||
1989 | { | 1345 | { |
1990 | if (!_characters.Contains(chr)) | 1346 | lock (_characters) |
1991 | { | ||
1992 | _characters.Add(chr); | ||
1993 | |||
1994 | // m_log.DebugFormat( | ||
1995 | // "[ODE SCENE]: Adding physics character {0} {1} to physics scene {2}. Count now {3}", | ||
1996 | // chr.Name, chr.LocalID, Name, _characters.Count); | ||
1997 | |||
1998 | if (chr.bad) | ||
1999 | m_log.ErrorFormat("[ODE SCENE]: Added BAD actor {0} to characters list", chr.m_uuid); | ||
2000 | } | ||
2001 | else | ||
2002 | { | 1347 | { |
2003 | m_log.ErrorFormat( | 1348 | if (!_characters.Contains(chr)) |
2004 | "[ODE SCENE]: Tried to add character {0} {1} but they are already in the set!", | 1349 | { |
2005 | chr.Name, chr.LocalID); | 1350 | _characters.Add(chr); |
1351 | if (chr.bad) | ||
1352 | m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid); | ||
1353 | } | ||
2006 | } | 1354 | } |
2007 | } | 1355 | } |
2008 | 1356 | ||
2009 | internal void RemoveCharacter(OdeCharacter chr) | 1357 | public void RemoveCharacter(OdeCharacter chr) |
2010 | { | 1358 | { |
2011 | if (_characters.Contains(chr)) | 1359 | lock (_characters) |
2012 | { | ||
2013 | _characters.Remove(chr); | ||
2014 | |||
2015 | // m_log.DebugFormat( | ||
2016 | // "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2}. Count now {3}", | ||
2017 | // chr.Name, chr.LocalID, Name, _characters.Count); | ||
2018 | } | ||
2019 | else | ||
2020 | { | 1360 | { |
2021 | m_log.ErrorFormat( | 1361 | if (_characters.Contains(chr)) |
2022 | "[ODE SCENE]: Tried to remove character {0} {1} but they are not in the list!", | 1362 | { |
2023 | chr.Name, chr.LocalID); | 1363 | _characters.Remove(chr); |
1364 | } | ||
2024 | } | 1365 | } |
2025 | } | 1366 | } |
2026 | 1367 | ||
2027 | private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation, | 1368 | public void BadCharacter(OdeCharacter chr) |
2028 | PrimitiveBaseShape pbs, bool isphysical, uint localID) | ||
2029 | { | 1369 | { |
2030 | Vector3 pos = position; | 1370 | lock (_badCharacter) |
2031 | Vector3 siz = size; | ||
2032 | Quaternion rot = rotation; | ||
2033 | |||
2034 | OdePrim newPrim; | ||
2035 | lock (OdeLock) | ||
2036 | { | 1371 | { |
2037 | newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical); | 1372 | if (!_badCharacter.Contains(chr)) |
2038 | 1373 | _badCharacter.Add(chr); | |
2039 | lock (_prims) | ||
2040 | _prims.Add(newPrim); | ||
2041 | } | 1374 | } |
2042 | newPrim.LocalID = localID; | ||
2043 | return newPrim; | ||
2044 | } | ||
2045 | |||
2046 | /// <summary> | ||
2047 | /// Make this prim subject to physics. | ||
2048 | /// </summary> | ||
2049 | /// <param name="prim"></param> | ||
2050 | internal void ActivatePrim(OdePrim prim) | ||
2051 | { | ||
2052 | // adds active prim.. (ones that should be iterated over in collisions_optimized | ||
2053 | if (!_activeprims.Contains(prim)) | ||
2054 | _activeprims.Add(prim); | ||
2055 | //else | ||
2056 | // m_log.Warn("[PHYSICS]: Double Entry in _activeprims detected, potential crash immenent"); | ||
2057 | } | 1375 | } |
2058 | 1376 | ||
2059 | public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, | 1377 | public override void RemoveAvatar(PhysicsActor actor) |
2060 | Vector3 size, Quaternion rotation, bool isPhysical, uint localid) | ||
2061 | { | ||
2062 | // m_log.DebugFormat("[ODE SCENE]: Adding physics prim {0} {1} to physics scene {2}", primName, localid, Name); | ||
2063 | |||
2064 | return AddPrim(primName, position, size, rotation, pbs, isPhysical, localid); | ||
2065 | } | ||
2066 | |||
2067 | public override float TimeDilation | ||
2068 | { | ||
2069 | get { return m_timeDilation; } | ||
2070 | } | ||
2071 | |||
2072 | public override bool SupportsNINJAJoints | ||
2073 | { | ||
2074 | get { return m_NINJA_physics_joints_enabled; } | ||
2075 | } | ||
2076 | |||
2077 | // internal utility function: must be called within a lock (OdeLock) | ||
2078 | private void InternalAddActiveJoint(PhysicsJoint joint) | ||
2079 | { | ||
2080 | activeJoints.Add(joint); | ||
2081 | SOPName_to_activeJoint.Add(joint.ObjectNameInScene, joint); | ||
2082 | } | ||
2083 | |||
2084 | // internal utility function: must be called within a lock (OdeLock) | ||
2085 | private void InternalAddPendingJoint(OdePhysicsJoint joint) | ||
2086 | { | ||
2087 | pendingJoints.Add(joint); | ||
2088 | SOPName_to_pendingJoint.Add(joint.ObjectNameInScene, joint); | ||
2089 | } | ||
2090 | |||
2091 | // internal utility function: must be called within a lock (OdeLock) | ||
2092 | private void InternalRemovePendingJoint(PhysicsJoint joint) | ||
2093 | { | 1378 | { |
2094 | pendingJoints.Remove(joint); | 1379 | //m_log.Debug("[PHYSICS]:ODELOCK"); |
2095 | SOPName_to_pendingJoint.Remove(joint.ObjectNameInScene); | 1380 | ((OdeCharacter) actor).Destroy(); |
2096 | } | 1381 | } |
2097 | 1382 | ||
2098 | // internal utility function: must be called within a lock (OdeLock) | ||
2099 | private void InternalRemoveActiveJoint(PhysicsJoint joint) | ||
2100 | { | ||
2101 | activeJoints.Remove(joint); | ||
2102 | SOPName_to_activeJoint.Remove(joint.ObjectNameInScene); | ||
2103 | } | ||
2104 | 1383 | ||
2105 | public override void DumpJointInfo() | 1384 | public void addActivePrim(OdePrim activatePrim) |
2106 | { | 1385 | { |
2107 | string hdr = "[NINJA] JOINTINFO: "; | 1386 | // adds active prim.. |
2108 | foreach (PhysicsJoint j in pendingJoints) | 1387 | lock (_activeprims) |
2109 | { | ||
2110 | m_log.Debug(hdr + " pending joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams); | ||
2111 | } | ||
2112 | m_log.Debug(hdr + pendingJoints.Count + " total pending joints"); | ||
2113 | foreach (string jointName in SOPName_to_pendingJoint.Keys) | ||
2114 | { | ||
2115 | m_log.Debug(hdr + " pending joints dict contains Name: " + jointName); | ||
2116 | } | ||
2117 | m_log.Debug(hdr + SOPName_to_pendingJoint.Keys.Count + " total pending joints dict entries"); | ||
2118 | foreach (PhysicsJoint j in activeJoints) | ||
2119 | { | ||
2120 | m_log.Debug(hdr + " active joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams); | ||
2121 | } | ||
2122 | m_log.Debug(hdr + activeJoints.Count + " total active joints"); | ||
2123 | foreach (string jointName in SOPName_to_activeJoint.Keys) | ||
2124 | { | ||
2125 | m_log.Debug(hdr + " active joints dict contains Name: " + jointName); | ||
2126 | } | ||
2127 | m_log.Debug(hdr + SOPName_to_activeJoint.Keys.Count + " total active joints dict entries"); | ||
2128 | |||
2129 | m_log.Debug(hdr + " Per-body joint connectivity information follows."); | ||
2130 | m_log.Debug(hdr + joints_connecting_actor.Keys.Count + " bodies are connected by joints."); | ||
2131 | foreach (string actorName in joints_connecting_actor.Keys) | ||
2132 | { | 1388 | { |
2133 | m_log.Debug(hdr + " Actor " + actorName + " has the following joints connecting it"); | 1389 | if (!_activeprims.Contains(activatePrim)) |
2134 | foreach (PhysicsJoint j in joints_connecting_actor[actorName]) | 1390 | _activeprims.Add(activatePrim); |
2135 | { | ||
2136 | m_log.Debug(hdr + " * joint Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams); | ||
2137 | } | ||
2138 | m_log.Debug(hdr + joints_connecting_actor[actorName].Count + " connecting joints total for this actor"); | ||
2139 | } | 1391 | } |
2140 | } | 1392 | } |
2141 | 1393 | ||
2142 | public override void RequestJointDeletion(string ObjectNameInScene) | 1394 | public void addActiveGroups(OdePrim activatePrim) |
2143 | { | 1395 | { |
2144 | lock (externalJointRequestsLock) | 1396 | lock (_activegroups) |
2145 | { | 1397 | { |
2146 | if (!requestedJointsToBeDeleted.Contains(ObjectNameInScene)) // forbid same deletion request from entering twice to prevent spurious deletions processed asynchronously | 1398 | if (!_activegroups.Contains(activatePrim)) |
2147 | { | 1399 | _activegroups.Add(activatePrim); |
2148 | requestedJointsToBeDeleted.Add(ObjectNameInScene); | ||
2149 | } | ||
2150 | } | 1400 | } |
2151 | } | 1401 | } |
2152 | 1402 | ||
2153 | private void DeleteRequestedJoints() | 1403 | private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation, |
1404 | PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID) | ||
2154 | { | 1405 | { |
2155 | List<string> myRequestedJointsToBeDeleted; | 1406 | OdePrim newPrim; |
2156 | lock (externalJointRequestsLock) | 1407 | lock (OdeLock) |
2157 | { | ||
2158 | // make a local copy of the shared list for processing (threading issues) | ||
2159 | myRequestedJointsToBeDeleted = new List<string>(requestedJointsToBeDeleted); | ||
2160 | } | ||
2161 | |||
2162 | foreach (string jointName in myRequestedJointsToBeDeleted) | ||
2163 | { | ||
2164 | lock (OdeLock) | ||
2165 | { | ||
2166 | //m_log.Debug("[NINJA] trying to deleting requested joint " + jointName); | ||
2167 | if (SOPName_to_activeJoint.ContainsKey(jointName) || SOPName_to_pendingJoint.ContainsKey(jointName)) | ||
2168 | { | ||
2169 | OdePhysicsJoint joint = null; | ||
2170 | if (SOPName_to_activeJoint.ContainsKey(jointName)) | ||
2171 | { | ||
2172 | joint = SOPName_to_activeJoint[jointName] as OdePhysicsJoint; | ||
2173 | InternalRemoveActiveJoint(joint); | ||
2174 | } | ||
2175 | else if (SOPName_to_pendingJoint.ContainsKey(jointName)) | ||
2176 | { | ||
2177 | joint = SOPName_to_pendingJoint[jointName] as OdePhysicsJoint; | ||
2178 | InternalRemovePendingJoint(joint); | ||
2179 | } | ||
2180 | |||
2181 | if (joint != null) | ||
2182 | { | ||
2183 | //m_log.Debug("joint.BodyNames.Count is " + joint.BodyNames.Count + " and contents " + joint.BodyNames); | ||
2184 | for (int iBodyName = 0; iBodyName < 2; iBodyName++) | ||
2185 | { | ||
2186 | string bodyName = joint.BodyNames[iBodyName]; | ||
2187 | if (bodyName != "NULL") | ||
2188 | { | ||
2189 | joints_connecting_actor[bodyName].Remove(joint); | ||
2190 | if (joints_connecting_actor[bodyName].Count == 0) | ||
2191 | { | ||
2192 | joints_connecting_actor.Remove(bodyName); | ||
2193 | } | ||
2194 | } | ||
2195 | } | ||
2196 | |||
2197 | DoJointDeactivated(joint); | ||
2198 | if (joint.jointID != IntPtr.Zero) | ||
2199 | { | ||
2200 | d.JointDestroy(joint.jointID); | ||
2201 | joint.jointID = IntPtr.Zero; | ||
2202 | //DoJointErrorMessage(joint, "successfully destroyed joint " + jointName); | ||
2203 | } | ||
2204 | else | ||
2205 | { | ||
2206 | //m_log.Warn("[NINJA] Ignoring re-request to destroy joint " + jointName); | ||
2207 | } | ||
2208 | } | ||
2209 | else | ||
2210 | { | ||
2211 | // DoJointErrorMessage(joint, "coult not find joint to destroy based on name " + jointName); | ||
2212 | } | ||
2213 | } | ||
2214 | else | ||
2215 | { | ||
2216 | // DoJointErrorMessage(joint, "WARNING - joint removal failed, joint " + jointName); | ||
2217 | } | ||
2218 | } | ||
2219 | } | ||
2220 | |||
2221 | // remove processed joints from the shared list | ||
2222 | lock (externalJointRequestsLock) | ||
2223 | { | 1408 | { |
2224 | foreach (string jointName in myRequestedJointsToBeDeleted) | 1409 | newPrim = new OdePrim(name, this, position, size, rotation, pbs, isphysical, isPhantom, shapeType, localID); |
2225 | { | 1410 | lock (_prims) |
2226 | requestedJointsToBeDeleted.Remove(jointName); | 1411 | _prims.Add(newPrim); |
2227 | } | ||
2228 | } | 1412 | } |
1413 | return newPrim; | ||
2229 | } | 1414 | } |
2230 | 1415 | ||
2231 | // for pending joints we don't know if their associated bodies exist yet or not. | 1416 | public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, |
2232 | // the joint is actually created during processing of the taints | 1417 | Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid) |
2233 | private void CreateRequestedJoints() | ||
2234 | { | 1418 | { |
2235 | List<PhysicsJoint> myRequestedJointsToBeCreated; | 1419 | return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, 0 , localid); |
2236 | lock (externalJointRequestsLock) | ||
2237 | { | ||
2238 | // make a local copy of the shared list for processing (threading issues) | ||
2239 | myRequestedJointsToBeCreated = new List<PhysicsJoint>(requestedJointsToBeCreated); | ||
2240 | } | ||
2241 | |||
2242 | foreach (PhysicsJoint joint in myRequestedJointsToBeCreated) | ||
2243 | { | ||
2244 | lock (OdeLock) | ||
2245 | { | ||
2246 | if (SOPName_to_pendingJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_pendingJoint[joint.ObjectNameInScene] != null) | ||
2247 | { | ||
2248 | DoJointErrorMessage(joint, "WARNING: ignoring request to re-add already pending joint Name:" + joint.ObjectNameInScene + " type:" + joint.Type + " parms: " + joint.RawParams + " pos: " + joint.Position + " rot:" + joint.Rotation); | ||
2249 | continue; | ||
2250 | } | ||
2251 | if (SOPName_to_activeJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_activeJoint[joint.ObjectNameInScene] != null) | ||
2252 | { | ||
2253 | DoJointErrorMessage(joint, "WARNING: ignoring request to re-add already active joint Name:" + joint.ObjectNameInScene + " type:" + joint.Type + " parms: " + joint.RawParams + " pos: " + joint.Position + " rot:" + joint.Rotation); | ||
2254 | continue; | ||
2255 | } | ||
2256 | |||
2257 | InternalAddPendingJoint(joint as OdePhysicsJoint); | ||
2258 | |||
2259 | if (joint.BodyNames.Count >= 2) | ||
2260 | { | ||
2261 | for (int iBodyName = 0; iBodyName < 2; iBodyName++) | ||
2262 | { | ||
2263 | string bodyName = joint.BodyNames[iBodyName]; | ||
2264 | if (bodyName != "NULL") | ||
2265 | { | ||
2266 | if (!joints_connecting_actor.ContainsKey(bodyName)) | ||
2267 | { | ||
2268 | joints_connecting_actor.Add(bodyName, new List<PhysicsJoint>()); | ||
2269 | } | ||
2270 | joints_connecting_actor[bodyName].Add(joint); | ||
2271 | } | ||
2272 | } | ||
2273 | } | ||
2274 | } | ||
2275 | } | ||
2276 | |||
2277 | // remove processed joints from shared list | ||
2278 | lock (externalJointRequestsLock) | ||
2279 | { | ||
2280 | foreach (PhysicsJoint joint in myRequestedJointsToBeCreated) | ||
2281 | { | ||
2282 | requestedJointsToBeCreated.Remove(joint); | ||
2283 | } | ||
2284 | } | ||
2285 | } | 1420 | } |
2286 | 1421 | ||
2287 | /// <summary> | ||
2288 | /// Add a request for joint creation. | ||
2289 | /// </summary> | ||
2290 | /// <remarks> | ||
2291 | /// this joint will just be added to a waiting list that is NOT processed during the main | ||
2292 | /// Simulate() loop (to avoid deadlocks). After Simulate() is finished, we handle unprocessed joint requests. | ||
2293 | /// </remarks> | ||
2294 | /// <param name="objectNameInScene"></param> | ||
2295 | /// <param name="jointType"></param> | ||
2296 | /// <param name="position"></param> | ||
2297 | /// <param name="rotation"></param> | ||
2298 | /// <param name="parms"></param> | ||
2299 | /// <param name="bodyNames"></param> | ||
2300 | /// <param name="trackedBodyName"></param> | ||
2301 | /// <param name="localRotation"></param> | ||
2302 | /// <returns></returns> | ||
2303 | public override PhysicsJoint RequestJointCreation( | ||
2304 | string objectNameInScene, PhysicsJointType jointType, Vector3 position, | ||
2305 | Quaternion rotation, string parms, List<string> bodyNames, string trackedBodyName, Quaternion localRotation) | ||
2306 | { | ||
2307 | OdePhysicsJoint joint = new OdePhysicsJoint(); | ||
2308 | joint.ObjectNameInScene = objectNameInScene; | ||
2309 | joint.Type = jointType; | ||
2310 | joint.Position = position; | ||
2311 | joint.Rotation = rotation; | ||
2312 | joint.RawParams = parms; | ||
2313 | joint.BodyNames = new List<string>(bodyNames); | ||
2314 | joint.TrackedBodyName = trackedBodyName; | ||
2315 | joint.LocalRotation = localRotation; | ||
2316 | joint.jointID = IntPtr.Zero; | ||
2317 | joint.ErrorMessageCount = 0; | ||
2318 | |||
2319 | lock (externalJointRequestsLock) | ||
2320 | { | ||
2321 | if (!requestedJointsToBeCreated.Contains(joint)) // forbid same creation request from entering twice | ||
2322 | { | ||
2323 | requestedJointsToBeCreated.Add(joint); | ||
2324 | } | ||
2325 | } | ||
2326 | |||
2327 | return joint; | ||
2328 | } | ||
2329 | 1422 | ||
2330 | private void RemoveAllJointsConnectedToActor(PhysicsActor actor) | 1423 | public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, |
1424 | Vector3 size, Quaternion rotation, bool isPhysical, uint localid) | ||
2331 | { | 1425 | { |
2332 | //m_log.Debug("RemoveAllJointsConnectedToActor: start"); | 1426 | return AddPrim(primName, position, size, rotation, pbs, isPhysical,false, 0, localid); |
2333 | if (actor.SOPName != null && joints_connecting_actor.ContainsKey(actor.SOPName) && joints_connecting_actor[actor.SOPName] != null) | ||
2334 | { | ||
2335 | List<PhysicsJoint> jointsToRemove = new List<PhysicsJoint>(); | ||
2336 | //TODO: merge these 2 loops (originally it was needed to avoid altering a list being iterated over, but it is no longer needed due to the joint request queue mechanism) | ||
2337 | foreach (PhysicsJoint j in joints_connecting_actor[actor.SOPName]) | ||
2338 | { | ||
2339 | jointsToRemove.Add(j); | ||
2340 | } | ||
2341 | foreach (PhysicsJoint j in jointsToRemove) | ||
2342 | { | ||
2343 | //m_log.Debug("RemoveAllJointsConnectedToActor: about to request deletion of " + j.ObjectNameInScene); | ||
2344 | RequestJointDeletion(j.ObjectNameInScene); | ||
2345 | //m_log.Debug("RemoveAllJointsConnectedToActor: done request deletion of " + j.ObjectNameInScene); | ||
2346 | j.TrackedBodyName = null; // *IMMEDIATELY* prevent any further movement of this joint (else a deleted actor might cause spurious tracking motion of the joint for a few frames, leading to the joint proxy object disappearing) | ||
2347 | } | ||
2348 | } | ||
2349 | } | 1427 | } |
2350 | 1428 | ||
2351 | public override void RemoveAllJointsConnectedToActorThreadLocked(PhysicsActor actor) | 1429 | public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, |
1430 | Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid) | ||
2352 | { | 1431 | { |
2353 | //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: start"); | 1432 | |
2354 | lock (OdeLock) | 1433 | return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid); |
2355 | { | ||
2356 | //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: got lock"); | ||
2357 | RemoveAllJointsConnectedToActor(actor); | ||
2358 | } | ||
2359 | } | 1434 | } |
2360 | 1435 | ||
2361 | // normally called from within OnJointMoved, which is called from within a lock (OdeLock) | 1436 | public void remActivePrim(OdePrim deactivatePrim) |
2362 | public override Vector3 GetJointAnchor(PhysicsJoint joint) | ||
2363 | { | 1437 | { |
2364 | Debug.Assert(joint.IsInPhysicsEngine); | 1438 | lock (_activeprims) |
2365 | d.Vector3 pos = new d.Vector3(); | ||
2366 | |||
2367 | if (!(joint is OdePhysicsJoint)) | ||
2368 | { | 1439 | { |
2369 | DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene); | 1440 | _activeprims.Remove(deactivatePrim); |
2370 | } | 1441 | } |
2371 | else | ||
2372 | { | ||
2373 | OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint; | ||
2374 | switch (odeJoint.Type) | ||
2375 | { | ||
2376 | case PhysicsJointType.Ball: | ||
2377 | d.JointGetBallAnchor(odeJoint.jointID, out pos); | ||
2378 | break; | ||
2379 | case PhysicsJointType.Hinge: | ||
2380 | d.JointGetHingeAnchor(odeJoint.jointID, out pos); | ||
2381 | break; | ||
2382 | } | ||
2383 | } | ||
2384 | return new Vector3(pos.X, pos.Y, pos.Z); | ||
2385 | } | 1442 | } |
2386 | 1443 | public void remActiveGroup(OdePrim deactivatePrim) | |
2387 | /// <summary> | ||
2388 | /// Get joint axis. | ||
2389 | /// </summary> | ||
2390 | /// <remarks> | ||
2391 | /// normally called from within OnJointMoved, which is called from within a lock (OdeLock) | ||
2392 | /// WARNING: ODE sometimes returns <0,0,0> as the joint axis! Therefore this function | ||
2393 | /// appears to be unreliable. Fortunately we can compute the joint axis ourselves by | ||
2394 | /// keeping track of the joint's original orientation relative to one of the involved bodies. | ||
2395 | /// </remarks> | ||
2396 | /// <param name="joint"></param> | ||
2397 | /// <returns></returns> | ||
2398 | public override Vector3 GetJointAxis(PhysicsJoint joint) | ||
2399 | { | 1444 | { |
2400 | Debug.Assert(joint.IsInPhysicsEngine); | 1445 | lock (_activegroups) |
2401 | d.Vector3 axis = new d.Vector3(); | ||
2402 | |||
2403 | if (!(joint is OdePhysicsJoint)) | ||
2404 | { | 1446 | { |
2405 | DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene); | 1447 | _activegroups.Remove(deactivatePrim); |
2406 | } | 1448 | } |
2407 | else | ||
2408 | { | ||
2409 | OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint; | ||
2410 | switch (odeJoint.Type) | ||
2411 | { | ||
2412 | case PhysicsJointType.Ball: | ||
2413 | DoJointErrorMessage(joint, "warning - axis requested for ball joint: " + joint.ObjectNameInScene); | ||
2414 | break; | ||
2415 | case PhysicsJointType.Hinge: | ||
2416 | d.JointGetHingeAxis(odeJoint.jointID, out axis); | ||
2417 | break; | ||
2418 | } | ||
2419 | } | ||
2420 | return new Vector3(axis.X, axis.Y, axis.Z); | ||
2421 | } | ||
2422 | |||
2423 | /// <summary> | ||
2424 | /// Stop this prim being subject to physics | ||
2425 | /// </summary> | ||
2426 | /// <param name="prim"></param> | ||
2427 | internal void DeactivatePrim(OdePrim prim) | ||
2428 | { | ||
2429 | _activeprims.Remove(prim); | ||
2430 | } | 1449 | } |
2431 | 1450 | ||
2432 | public override void RemovePrim(PhysicsActor prim) | 1451 | public override void RemovePrim(PhysicsActor prim) |
@@ -2435,16 +1454,14 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
2435 | // removed in the next physics simulate pass. | 1454 | // removed in the next physics simulate pass. |
2436 | if (prim is OdePrim) | 1455 | if (prim is OdePrim) |
2437 | { | 1456 | { |
2438 | lock (OdeLock) | 1457 | // lock (OdeLock) |
2439 | { | 1458 | { |
2440 | OdePrim p = (OdePrim) prim; | 1459 | |
2441 | 1460 | OdePrim p = (OdePrim)prim; | |
2442 | p.setPrimForRemoval(); | 1461 | p.setPrimForRemoval(); |
2443 | AddPhysicsActorTaint(prim); | ||
2444 | } | 1462 | } |
2445 | } | 1463 | } |
2446 | } | 1464 | } |
2447 | |||
2448 | /// <summary> | 1465 | /// <summary> |
2449 | /// This is called from within simulate but outside the locked portion | 1466 | /// This is called from within simulate but outside the locked portion |
2450 | /// We need to do our own locking here | 1467 | /// We need to do our own locking here |
@@ -2457,772 +1474,473 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
2457 | /// that the space was using. | 1474 | /// that the space was using. |
2458 | /// </summary> | 1475 | /// </summary> |
2459 | /// <param name="prim"></param> | 1476 | /// <param name="prim"></param> |
2460 | internal void RemovePrimThreadLocked(OdePrim prim) | 1477 | public void RemovePrimThreadLocked(OdePrim prim) |
2461 | { | 1478 | { |
2462 | // m_log.DebugFormat("[ODE SCENE]: Removing physical prim {0} {1}", prim.Name, prim.LocalID); | 1479 | //Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName); |
2463 | |||
2464 | lock (prim) | 1480 | lock (prim) |
2465 | { | 1481 | { |
2466 | RemoveCollisionEventReporting(prim); | 1482 | // RemoveCollisionEventReporting(prim); |
2467 | 1483 | lock (_prims) | |
2468 | if (prim.prim_geom != IntPtr.Zero) | 1484 | _prims.Remove(prim); |
2469 | { | ||
2470 | prim.ResetTaints(); | ||
2471 | |||
2472 | if (prim.IsPhysical) | ||
2473 | { | ||
2474 | prim.disableBody(); | ||
2475 | if (prim.childPrim) | ||
2476 | { | ||
2477 | prim.childPrim = false; | ||
2478 | prim.Body = IntPtr.Zero; | ||
2479 | prim.m_disabled = true; | ||
2480 | prim.IsPhysical = false; | ||
2481 | } | ||
2482 | |||
2483 | |||
2484 | } | ||
2485 | // we don't want to remove the main space | ||
2486 | |||
2487 | // If the geometry is in the targetspace, remove it from the target space | ||
2488 | //m_log.Warn(prim.m_targetSpace); | ||
2489 | |||
2490 | //if (prim.m_targetSpace != IntPtr.Zero) | ||
2491 | //{ | ||
2492 | //if (d.SpaceQuery(prim.m_targetSpace, prim.prim_geom)) | ||
2493 | //{ | ||
2494 | |||
2495 | //if (d.GeomIsSpace(prim.m_targetSpace)) | ||
2496 | //{ | ||
2497 | //waitForSpaceUnlock(prim.m_targetSpace); | ||
2498 | //d.SpaceRemove(prim.m_targetSpace, prim.prim_geom); | ||
2499 | prim.m_targetSpace = IntPtr.Zero; | ||
2500 | //} | ||
2501 | //else | ||
2502 | //{ | ||
2503 | // m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" + | ||
2504 | //((OdePrim)prim).m_targetSpace.ToString()); | ||
2505 | //} | ||
2506 | |||
2507 | //} | ||
2508 | //} | ||
2509 | //m_log.Warn(prim.prim_geom); | ||
2510 | |||
2511 | if (!prim.RemoveGeom()) | ||
2512 | m_log.Warn("[ODE SCENE]: Unable to remove prim from physics scene"); | ||
2513 | |||
2514 | lock (_prims) | ||
2515 | _prims.Remove(prim); | ||
2516 | |||
2517 | //If there are no more geometries in the sub-space, we don't need it in the main space anymore | ||
2518 | //if (d.SpaceGetNumGeoms(prim.m_targetSpace) == 0) | ||
2519 | //{ | ||
2520 | //if (prim.m_targetSpace != null) | ||
2521 | //{ | ||
2522 | //if (d.GeomIsSpace(prim.m_targetSpace)) | ||
2523 | //{ | ||
2524 | //waitForSpaceUnlock(prim.m_targetSpace); | ||
2525 | //d.SpaceRemove(space, prim.m_targetSpace); | ||
2526 | // free up memory used by the space. | ||
2527 | //d.SpaceDestroy(prim.m_targetSpace); | ||
2528 | //int[] xyspace = calculateSpaceArrayItemFromPos(prim.Position); | ||
2529 | //resetSpaceArrayItemToZero(xyspace[0], xyspace[1]); | ||
2530 | //} | ||
2531 | //else | ||
2532 | //{ | ||
2533 | //m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" + | ||
2534 | //((OdePrim) prim).m_targetSpace.ToString()); | ||
2535 | //} | ||
2536 | //} | ||
2537 | //} | ||
2538 | |||
2539 | if (SupportsNINJAJoints) | ||
2540 | RemoveAllJointsConnectedToActorThreadLocked(prim); | ||
2541 | } | ||
2542 | } | 1485 | } |
2543 | } | ||
2544 | 1486 | ||
2545 | #endregion | 1487 | } |
2546 | 1488 | ||
2547 | #region Space Separation Calculation | 1489 | public bool havePrim(OdePrim prm) |
1490 | { | ||
1491 | lock (_prims) | ||
1492 | return _prims.Contains(prm); | ||
1493 | } | ||
2548 | 1494 | ||
2549 | /// <summary> | 1495 | public bool haveActor(PhysicsActor actor) |
2550 | /// Takes a space pointer and zeros out the array we're using to hold the spaces | ||
2551 | /// </summary> | ||
2552 | /// <param name="pSpace"></param> | ||
2553 | private void resetSpaceArrayItemToZero(IntPtr pSpace) | ||
2554 | { | 1496 | { |
2555 | for (int x = 0; x < staticPrimspace.GetLength(0); x++) | 1497 | if (actor is OdePrim) |
2556 | { | 1498 | { |
2557 | for (int y = 0; y < staticPrimspace.GetLength(1); y++) | 1499 | lock (_prims) |
2558 | { | 1500 | return _prims.Contains((OdePrim)actor); |
2559 | if (staticPrimspace[x, y] == pSpace) | 1501 | } |
2560 | staticPrimspace[x, y] = IntPtr.Zero; | 1502 | else if (actor is OdeCharacter) |
2561 | } | 1503 | { |
1504 | lock (_characters) | ||
1505 | return _characters.Contains((OdeCharacter)actor); | ||
2562 | } | 1506 | } |
1507 | return false; | ||
2563 | } | 1508 | } |
2564 | 1509 | ||
2565 | // private void resetSpaceArrayItemToZero(int arrayitemX, int arrayitemY) | 1510 | #endregion |
2566 | // { | 1511 | |
2567 | // staticPrimspace[arrayitemX, arrayitemY] = IntPtr.Zero; | 1512 | #region Space Separation Calculation |
2568 | // } | ||
2569 | 1513 | ||
2570 | /// <summary> | 1514 | /// <summary> |
2571 | /// Called when a static prim moves. Allocates a space for the prim based on its position | 1515 | /// Called when a static prim moves or becomes static |
1516 | /// Places the prim in a space one the static sub-spaces grid | ||
2572 | /// </summary> | 1517 | /// </summary> |
2573 | /// <param name="geom">the pointer to the geom that moved</param> | 1518 | /// <param name="geom">the pointer to the geom that moved</param> |
2574 | /// <param name="pos">the position that the geom moved to</param> | 1519 | /// <param name="pos">the position that the geom moved to</param> |
2575 | /// <param name="currentspace">a pointer to the space it was in before it was moved.</param> | 1520 | /// <param name="currentspace">a pointer to the space it was in before it was moved.</param> |
2576 | /// <returns>a pointer to the new space it's in</returns> | 1521 | /// <returns>a pointer to the new space it's in</returns> |
2577 | internal IntPtr recalculateSpaceForGeom(IntPtr geom, Vector3 pos, IntPtr currentspace) | 1522 | public IntPtr MoveGeomToStaticSpace(IntPtr geom, Vector3 pos, IntPtr currentspace) |
2578 | { | 1523 | { |
2579 | // Called from setting the Position and Size of an ODEPrim so | 1524 | // moves a prim into another static sub-space or from another space into a static sub-space |
1525 | |||
1526 | // Called ODEPrim so | ||
2580 | // it's already in locked space. | 1527 | // it's already in locked space. |
2581 | 1528 | ||
2582 | // we don't want to remove the main space | 1529 | if (geom == IntPtr.Zero) // shouldn't happen |
2583 | // we don't need to test physical here because this function should | 1530 | return IntPtr.Zero; |
2584 | // never be called if the prim is physical(active) | ||
2585 | 1531 | ||
2586 | // All physical prim end up in the root space | 1532 | // get the static sub-space for current position |
2587 | //Thread.Sleep(20); | 1533 | IntPtr newspace = calculateSpaceForGeom(pos); |
2588 | if (currentspace != space) | 1534 | |
1535 | if (newspace == currentspace) // if we are there all done | ||
1536 | return newspace; | ||
1537 | |||
1538 | // else remove it from its current space | ||
1539 | if (currentspace != IntPtr.Zero && d.SpaceQuery(currentspace, geom)) | ||
2589 | { | 1540 | { |
2590 | //m_log.Info("[SPACE]: C:" + currentspace.ToString() + " g:" + geom.ToString()); | 1541 | if (d.GeomIsSpace(currentspace)) |
2591 | //if (currentspace == IntPtr.Zero) | ||
2592 | //{ | ||
2593 | //int adfadf = 0; | ||
2594 | //} | ||
2595 | if (d.SpaceQuery(currentspace, geom) && currentspace != IntPtr.Zero) | ||
2596 | { | 1542 | { |
2597 | if (d.GeomIsSpace(currentspace)) | 1543 | waitForSpaceUnlock(currentspace); |
2598 | { | 1544 | d.SpaceRemove(currentspace, geom); |
2599 | // waitForSpaceUnlock(currentspace); | 1545 | |
2600 | d.SpaceRemove(currentspace, geom); | 1546 | if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0) |
2601 | } | ||
2602 | else | ||
2603 | { | 1547 | { |
2604 | m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" + currentspace + | 1548 | d.SpaceDestroy(currentspace); |
2605 | " Geom:" + geom); | ||
2606 | } | 1549 | } |
2607 | } | 1550 | } |
2608 | else | 1551 | else |
2609 | { | 1552 | { |
2610 | IntPtr sGeomIsIn = d.GeomGetSpace(geom); | 1553 | m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace + |
2611 | if (sGeomIsIn != IntPtr.Zero) | 1554 | " Geom:" + geom); |
2612 | { | ||
2613 | if (d.GeomIsSpace(currentspace)) | ||
2614 | { | ||
2615 | // waitForSpaceUnlock(sGeomIsIn); | ||
2616 | d.SpaceRemove(sGeomIsIn, geom); | ||
2617 | } | ||
2618 | else | ||
2619 | { | ||
2620 | m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" + | ||
2621 | sGeomIsIn + " Geom:" + geom); | ||
2622 | } | ||
2623 | } | ||
2624 | } | ||
2625 | |||
2626 | //If there are no more geometries in the sub-space, we don't need it in the main space anymore | ||
2627 | if (d.SpaceGetNumGeoms(currentspace) == 0) | ||
2628 | { | ||
2629 | if (currentspace != IntPtr.Zero) | ||
2630 | { | ||
2631 | if (d.GeomIsSpace(currentspace)) | ||
2632 | { | ||
2633 | // waitForSpaceUnlock(currentspace); | ||
2634 | // waitForSpaceUnlock(space); | ||
2635 | d.SpaceRemove(space, currentspace); | ||
2636 | // free up memory used by the space. | ||
2637 | |||
2638 | //d.SpaceDestroy(currentspace); | ||
2639 | resetSpaceArrayItemToZero(currentspace); | ||
2640 | } | ||
2641 | else | ||
2642 | { | ||
2643 | m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" + | ||
2644 | currentspace + " Geom:" + geom); | ||
2645 | } | ||
2646 | } | ||
2647 | } | 1555 | } |
2648 | } | 1556 | } |
2649 | else | 1557 | else // odd currentspace is null or doesn't contain the geom? lets try the geom ideia of current space |
2650 | { | 1558 | { |
2651 | // this is a physical object that got disabled. ;.; | 1559 | currentspace = d.GeomGetSpace(geom); |
2652 | if (currentspace != IntPtr.Zero && geom != IntPtr.Zero) | 1560 | if (currentspace != IntPtr.Zero) |
2653 | { | 1561 | { |
2654 | if (d.SpaceQuery(currentspace, geom)) | 1562 | if (d.GeomIsSpace(currentspace)) |
2655 | { | ||
2656 | if (d.GeomIsSpace(currentspace)) | ||
2657 | { | ||
2658 | // waitForSpaceUnlock(currentspace); | ||
2659 | d.SpaceRemove(currentspace, geom); | ||
2660 | } | ||
2661 | else | ||
2662 | { | ||
2663 | m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" + | ||
2664 | currentspace + " Geom:" + geom); | ||
2665 | } | ||
2666 | } | ||
2667 | else | ||
2668 | { | 1563 | { |
2669 | IntPtr sGeomIsIn = d.GeomGetSpace(geom); | 1564 | waitForSpaceUnlock(currentspace); |
2670 | if (sGeomIsIn != IntPtr.Zero) | 1565 | d.SpaceRemove(currentspace, geom); |
1566 | |||
1567 | if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0) | ||
2671 | { | 1568 | { |
2672 | if (d.GeomIsSpace(sGeomIsIn)) | 1569 | d.SpaceDestroy(currentspace); |
2673 | { | ||
2674 | // waitForSpaceUnlock(sGeomIsIn); | ||
2675 | d.SpaceRemove(sGeomIsIn, geom); | ||
2676 | } | ||
2677 | else | ||
2678 | { | ||
2679 | m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" + | ||
2680 | sGeomIsIn + " Geom:" + geom); | ||
2681 | } | ||
2682 | } | 1570 | } |
1571 | |||
2683 | } | 1572 | } |
2684 | } | 1573 | } |
2685 | } | 1574 | } |
2686 | 1575 | ||
2687 | // The routines in the Position and Size sections do the 'inserting' into the space, | 1576 | // put the geom in the newspace |
2688 | // so all we have to do is make sure that the space that we're putting the prim into | 1577 | waitForSpaceUnlock(newspace); |
2689 | // is in the 'main' space. | 1578 | d.SpaceAdd(newspace, geom); |
2690 | int[] iprimspaceArrItem = calculateSpaceArrayItemFromPos(pos); | ||
2691 | IntPtr newspace = calculateSpaceForGeom(pos); | ||
2692 | |||
2693 | if (newspace == IntPtr.Zero) | ||
2694 | { | ||
2695 | newspace = createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]); | ||
2696 | d.HashSpaceSetLevels(newspace, smallHashspaceLow, smallHashspaceHigh); | ||
2697 | } | ||
2698 | 1579 | ||
1580 | // let caller know this newspace | ||
2699 | return newspace; | 1581 | return newspace; |
2700 | } | 1582 | } |
2701 | 1583 | ||
2702 | /// <summary> | 1584 | /// <summary> |
2703 | /// Creates a new space at X Y | ||
2704 | /// </summary> | ||
2705 | /// <param name="iprimspaceArrItemX"></param> | ||
2706 | /// <param name="iprimspaceArrItemY"></param> | ||
2707 | /// <returns>A pointer to the created space</returns> | ||
2708 | internal IntPtr createprimspace(int iprimspaceArrItemX, int iprimspaceArrItemY) | ||
2709 | { | ||
2710 | // creating a new space for prim and inserting it into main space. | ||
2711 | staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY] = d.HashSpaceCreate(IntPtr.Zero); | ||
2712 | d.GeomSetCategoryBits(staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY], (int)CollisionCategories.Space); | ||
2713 | // waitForSpaceUnlock(space); | ||
2714 | d.SpaceSetSublevel(space, 1); | ||
2715 | d.SpaceAdd(space, staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY]); | ||
2716 | |||
2717 | return staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY]; | ||
2718 | } | ||
2719 | |||
2720 | /// <summary> | ||
2721 | /// Calculates the space the prim should be in by its position | 1585 | /// Calculates the space the prim should be in by its position |
2722 | /// </summary> | 1586 | /// </summary> |
2723 | /// <param name="pos"></param> | 1587 | /// <param name="pos"></param> |
2724 | /// <returns>a pointer to the space. This could be a new space or reused space.</returns> | 1588 | /// <returns>a pointer to the space. This could be a new space or reused space.</returns> |
2725 | internal IntPtr calculateSpaceForGeom(Vector3 pos) | 1589 | public IntPtr calculateSpaceForGeom(Vector3 pos) |
2726 | { | ||
2727 | int[] xyspace = calculateSpaceArrayItemFromPos(pos); | ||
2728 | //m_log.Info("[Physics]: Attempting to use arrayItem: " + xyspace[0].ToString() + "," + xyspace[1].ToString()); | ||
2729 | return staticPrimspace[xyspace[0], xyspace[1]]; | ||
2730 | } | ||
2731 | |||
2732 | /// <summary> | ||
2733 | /// Holds the space allocation logic | ||
2734 | /// </summary> | ||
2735 | /// <param name="pos"></param> | ||
2736 | /// <returns>an array item based on the position</returns> | ||
2737 | internal int[] calculateSpaceArrayItemFromPos(Vector3 pos) | ||
2738 | { | 1590 | { |
2739 | int[] returnint = new int[2]; | 1591 | int x, y; |
2740 | 1592 | ||
2741 | returnint[0] = (int) (pos.X/metersInSpace); | 1593 | if (pos.X < 0) |
1594 | return staticPrimspaceOffRegion[0]; | ||
2742 | 1595 | ||
2743 | if (returnint[0] > ((int) (259f/metersInSpace))) | 1596 | if (pos.Y < 0) |
2744 | returnint[0] = ((int) (259f/metersInSpace)); | 1597 | return staticPrimspaceOffRegion[2]; |
2745 | if (returnint[0] < 0) | ||
2746 | returnint[0] = 0; | ||
2747 | 1598 | ||
2748 | returnint[1] = (int) (pos.Y/metersInSpace); | 1599 | x = (int)(pos.X * spacesPerMeter); |
2749 | if (returnint[1] > ((int) (259f/metersInSpace))) | 1600 | if (x > spaceGridMaxX) |
2750 | returnint[1] = ((int) (259f/metersInSpace)); | 1601 | return staticPrimspaceOffRegion[1]; |
2751 | if (returnint[1] < 0) | 1602 | |
2752 | returnint[1] = 0; | 1603 | y = (int)(pos.Y * spacesPerMeter); |
1604 | if (y > spaceGridMaxY) | ||
1605 | return staticPrimspaceOffRegion[3]; | ||
2753 | 1606 | ||
2754 | return returnint; | 1607 | return staticPrimspace[x, y]; |
2755 | } | 1608 | } |
2756 | 1609 | ||
2757 | #endregion | 1610 | #endregion |
2758 | 1611 | ||
1612 | |||
2759 | /// <summary> | 1613 | /// <summary> |
2760 | /// Routine to figure out if we need to mesh this prim with our mesher | 1614 | /// Called to queue a change to a actor |
1615 | /// to use in place of old taint mechanism so changes do have a time sequence | ||
2761 | /// </summary> | 1616 | /// </summary> |
2762 | /// <param name="pbs"></param> | ||
2763 | /// <returns></returns> | ||
2764 | internal bool needsMeshing(PrimitiveBaseShape pbs) | ||
2765 | { | ||
2766 | // most of this is redundant now as the mesher will return null if it cant mesh a prim | ||
2767 | // but we still need to check for sculptie meshing being enabled so this is the most | ||
2768 | // convenient place to do it for now... | ||
2769 | |||
2770 | // //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f) | ||
2771 | // //m_log.Debug("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString()); | ||
2772 | int iPropertiesNotSupportedDefault = 0; | ||
2773 | |||
2774 | if (pbs.SculptEntry && !meshSculptedPrim) | ||
2775 | { | ||
2776 | #if SPAM | ||
2777 | m_log.Warn("NonMesh"); | ||
2778 | #endif | ||
2779 | return false; | ||
2780 | } | ||
2781 | |||
2782 | // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim | ||
2783 | if (!forceSimplePrimMeshing && !pbs.SculptEntry) | ||
2784 | { | ||
2785 | if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight) | ||
2786 | || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 | ||
2787 | && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z)) | ||
2788 | { | ||
2789 | |||
2790 | if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0 | ||
2791 | && pbs.ProfileHollow == 0 | ||
2792 | && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0 | ||
2793 | && pbs.PathBegin == 0 && pbs.PathEnd == 0 | ||
2794 | && pbs.PathTaperX == 0 && pbs.PathTaperY == 0 | ||
2795 | && pbs.PathScaleX == 100 && pbs.PathScaleY == 100 | ||
2796 | && pbs.PathShearX == 0 && pbs.PathShearY == 0) | ||
2797 | { | ||
2798 | #if SPAM | ||
2799 | m_log.Warn("NonMesh"); | ||
2800 | #endif | ||
2801 | return false; | ||
2802 | } | ||
2803 | } | ||
2804 | } | ||
2805 | |||
2806 | if (pbs.ProfileHollow != 0) | ||
2807 | iPropertiesNotSupportedDefault++; | ||
2808 | 1617 | ||
2809 | if ((pbs.PathBegin != 0) || pbs.PathEnd != 0) | 1618 | public void AddChange(PhysicsActor actor, changes what, Object arg) |
2810 | iPropertiesNotSupportedDefault++; | 1619 | { |
2811 | 1620 | ODEchangeitem item = new ODEchangeitem(); | |
2812 | if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0)) | 1621 | item.actor = actor; |
2813 | iPropertiesNotSupportedDefault++; | 1622 | item.what = what; |
2814 | 1623 | item.arg = arg; | |
2815 | if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0) | 1624 | ChangesQueue.Enqueue(item); |
2816 | iPropertiesNotSupportedDefault++; | ||
2817 | |||
2818 | if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100)) | ||
2819 | iPropertiesNotSupportedDefault++; | ||
2820 | |||
2821 | if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0)) | ||
2822 | iPropertiesNotSupportedDefault++; | ||
2823 | |||
2824 | if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight) | ||
2825 | iPropertiesNotSupportedDefault++; | ||
2826 | |||
2827 | if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X)) | ||
2828 | iPropertiesNotSupportedDefault++; | ||
2829 | |||
2830 | if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1) | ||
2831 | iPropertiesNotSupportedDefault++; | ||
2832 | |||
2833 | // test for torus | ||
2834 | if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square) | ||
2835 | { | ||
2836 | if (pbs.PathCurve == (byte)Extrusion.Curve1) | ||
2837 | { | ||
2838 | iPropertiesNotSupportedDefault++; | ||
2839 | } | ||
2840 | } | ||
2841 | else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle) | ||
2842 | { | ||
2843 | if (pbs.PathCurve == (byte)Extrusion.Straight) | ||
2844 | { | ||
2845 | iPropertiesNotSupportedDefault++; | ||
2846 | } | ||
2847 | |||
2848 | // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits | ||
2849 | else if (pbs.PathCurve == (byte)Extrusion.Curve1) | ||
2850 | { | ||
2851 | iPropertiesNotSupportedDefault++; | ||
2852 | } | ||
2853 | } | ||
2854 | else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle) | ||
2855 | { | ||
2856 | if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2) | ||
2857 | { | ||
2858 | iPropertiesNotSupportedDefault++; | ||
2859 | } | ||
2860 | } | ||
2861 | else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle) | ||
2862 | { | ||
2863 | if (pbs.PathCurve == (byte)Extrusion.Straight) | ||
2864 | { | ||
2865 | iPropertiesNotSupportedDefault++; | ||
2866 | } | ||
2867 | else if (pbs.PathCurve == (byte)Extrusion.Curve1) | ||
2868 | { | ||
2869 | iPropertiesNotSupportedDefault++; | ||
2870 | } | ||
2871 | } | ||
2872 | |||
2873 | if (pbs.SculptEntry && meshSculptedPrim) | ||
2874 | iPropertiesNotSupportedDefault++; | ||
2875 | |||
2876 | if (iPropertiesNotSupportedDefault == 0) | ||
2877 | { | ||
2878 | #if SPAM | ||
2879 | m_log.Warn("NonMesh"); | ||
2880 | #endif | ||
2881 | return false; | ||
2882 | } | ||
2883 | #if SPAM | ||
2884 | m_log.Debug("Mesh"); | ||
2885 | #endif | ||
2886 | return true; | ||
2887 | } | 1625 | } |
2888 | 1626 | ||
2889 | /// <summary> | 1627 | /// <summary> |
2890 | /// Called after our prim properties are set Scale, position etc. | 1628 | /// Called after our prim properties are set Scale, position etc. |
2891 | /// </summary> | ||
2892 | /// <remarks> | ||
2893 | /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex | 1629 | /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex |
2894 | /// This assures us that we have no race conditions | 1630 | /// This assures us that we have no race conditions |
2895 | /// </remarks> | 1631 | /// </summary> |
2896 | /// <param name="actor"></param> | 1632 | /// <param name="prim"></param> |
2897 | public override void AddPhysicsActorTaint(PhysicsActor actor) | 1633 | public override void AddPhysicsActorTaint(PhysicsActor prim) |
2898 | { | 1634 | { |
2899 | if (actor is OdePrim) | 1635 | } |
2900 | { | 1636 | |
2901 | OdePrim taintedprim = ((OdePrim)actor); | 1637 | // does all pending changes generated during region load process |
2902 | lock (_taintedPrims) | 1638 | public override void PrepareSimulation() |
2903 | _taintedPrims.Add(taintedprim); | 1639 | { |
2904 | } | 1640 | lock (OdeLock) |
2905 | else if (actor is OdeCharacter) | ||
2906 | { | 1641 | { |
2907 | OdeCharacter taintedchar = ((OdeCharacter)actor); | 1642 | if (world == IntPtr.Zero) |
2908 | lock (_taintedActors) | ||
2909 | { | 1643 | { |
2910 | _taintedActors.Add(taintedchar); | 1644 | ChangesQueue.Clear(); |
2911 | if (taintedchar.bad) | 1645 | return; |
2912 | m_log.ErrorFormat("[ODE SCENE]: Added BAD actor {0} to tainted actors", taintedchar.m_uuid); | 1646 | } |
1647 | |||
1648 | ODEchangeitem item; | ||
1649 | |||
1650 | int donechanges = 0; | ||
1651 | if (ChangesQueue.Count > 0) | ||
1652 | { | ||
1653 | m_log.InfoFormat("[ODE] start processing pending actor operations"); | ||
1654 | int tstart = Util.EnvironmentTickCount(); | ||
1655 | |||
1656 | while (ChangesQueue.Dequeue(out item)) | ||
1657 | { | ||
1658 | if (item.actor != null) | ||
1659 | { | ||
1660 | try | ||
1661 | { | ||
1662 | if (item.actor is OdeCharacter) | ||
1663 | ((OdeCharacter)item.actor).DoAChange(item.what, item.arg); | ||
1664 | else if (((OdePrim)item.actor).DoAChange(item.what, item.arg)) | ||
1665 | RemovePrimThreadLocked((OdePrim)item.actor); | ||
1666 | } | ||
1667 | catch | ||
1668 | { | ||
1669 | m_log.WarnFormat("[PHYSICS]: Operation failed for a actor {0} {1}", | ||
1670 | item.actor.Name, item.what.ToString()); | ||
1671 | } | ||
1672 | } | ||
1673 | donechanges++; | ||
1674 | } | ||
1675 | int time = Util.EnvironmentTickCountSubtract(tstart); | ||
1676 | m_log.InfoFormat("[ODE] finished {0} operations in {1}ms", donechanges, time); | ||
2913 | } | 1677 | } |
2914 | } | 1678 | } |
2915 | } | 1679 | } |
2916 | 1680 | ||
2917 | /// <summary> | 1681 | /// <summary> |
2918 | /// This is our main simulate loop | 1682 | /// This is our main simulate loop |
2919 | /// </summary> | ||
2920 | /// <remarks> | ||
2921 | /// It's thread locked by a Mutex in the scene. | 1683 | /// It's thread locked by a Mutex in the scene. |
2922 | /// It holds Collisions, it instructs ODE to step through the physical reactions | 1684 | /// It holds Collisions, it instructs ODE to step through the physical reactions |
2923 | /// It moves the objects around in memory | 1685 | /// It moves the objects around in memory |
2924 | /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup) | 1686 | /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup) |
2925 | /// </remarks> | 1687 | /// </summary> |
2926 | /// <param name="timeStep"></param> | 1688 | /// <param name="timeStep"></param> |
2927 | /// <returns>The number of frames simulated over that period.</returns> | 1689 | /// <returns></returns> |
2928 | public override float Simulate(float timeStep) | 1690 | public override float Simulate(float timeStep) |
2929 | { | 1691 | { |
2930 | if (!_worldInitialized) return 11f; | ||
2931 | 1692 | ||
2932 | int startFrameTick = CollectStats ? Util.EnvironmentTickCount() : 0; | 1693 | DateTime now = DateTime.UtcNow; |
2933 | int tempTick = 0, tempTick2 = 0; | 1694 | TimeSpan timedif = now - m_lastframe; |
1695 | m_lastframe = now; | ||
1696 | timeStep = (float)timedif.TotalSeconds; | ||
1697 | |||
1698 | // acumulate time so we can reduce error | ||
1699 | step_time += timeStep; | ||
1700 | |||
1701 | if (step_time < HalfOdeStep) | ||
1702 | return 0; | ||
2934 | 1703 | ||
2935 | if (framecount >= int.MaxValue) | 1704 | if (framecount < 0) |
2936 | framecount = 0; | 1705 | framecount = 0; |
2937 | 1706 | ||
2938 | framecount++; | 1707 | framecount++; |
2939 | 1708 | ||
2940 | float fps = 0; | 1709 | int curphysiteractions; |
2941 | 1710 | ||
2942 | float timeLeft = timeStep; | 1711 | // if in trouble reduce step resolution |
2943 | 1712 | if (step_time >= m_SkipFramesAtms) | |
2944 | //m_log.Info(timeStep.ToString()); | 1713 | curphysiteractions = m_physicsiterations / 2; |
2945 | // step_time += timeSte | 1714 | else |
2946 | // | 1715 | curphysiteractions = m_physicsiterations; |
2947 | // // If We're loaded down by something else, | ||
2948 | // // or debugging with the Visual Studio project on pause | ||
2949 | // // skip a few frames to catch up gracefully. | ||
2950 | // // without shooting the physicsactors all over the place | ||
2951 | // | ||
2952 | // if (step_time >= m_SkipFramesAtms) | ||
2953 | // { | ||
2954 | // // Instead of trying to catch up, it'll do 5 physics frames only | ||
2955 | // step_time = ODE_STEPSIZE; | ||
2956 | // m_physicsiterations = 5; | ||
2957 | // } | ||
2958 | // else | ||
2959 | // { | ||
2960 | // m_physicsiterations = 10; | ||
2961 | // } | ||
2962 | |||
2963 | // We change _collisionEventPrimChanges to avoid locking _collisionEventPrim itself and causing potential | ||
2964 | // deadlock if the collision event tries to lock something else later on which is already locked by a | ||
2965 | // caller that is adding or removing the collision event. | ||
2966 | lock (m_collisionEventActorsChanges) | ||
2967 | { | ||
2968 | foreach (KeyValuePair<uint, PhysicsActor> kvp in m_collisionEventActorsChanges) | ||
2969 | { | ||
2970 | if (kvp.Value == null) | ||
2971 | m_collisionEventActors.Remove(kvp.Key); | ||
2972 | else | ||
2973 | m_collisionEventActors[kvp.Key] = kvp.Value; | ||
2974 | } | ||
2975 | 1716 | ||
2976 | m_collisionEventActorsChanges.Clear(); | 1717 | int nodeframes = 0; |
2977 | } | ||
2978 | 1718 | ||
2979 | if (SupportsNINJAJoints) | 1719 | // checkThread(); |
2980 | { | ||
2981 | DeleteRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks | ||
2982 | CreateRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks | ||
2983 | } | ||
2984 | 1720 | ||
2985 | lock (OdeLock) | 1721 | lock (SimulationLock) |
1722 | lock(OdeLock) | ||
2986 | { | 1723 | { |
2987 | // Process 10 frames if the sim is running normal.. | 1724 | if (world == IntPtr.Zero) |
2988 | // process 5 frames if the sim is running slow | ||
2989 | //try | ||
2990 | //{ | ||
2991 | //d.WorldSetQuickStepNumIterations(world, m_physicsiterations); | ||
2992 | //} | ||
2993 | //catch (StackOverflowException) | ||
2994 | //{ | ||
2995 | // m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation. Restarting the sim."); | ||
2996 | // ode.drelease(world); | ||
2997 | //base.TriggerPhysicsBasedRestart(); | ||
2998 | //} | ||
2999 | |||
3000 | // Figure out the Frames Per Second we're going at. | ||
3001 | //(step_time == 0.004f, there's 250 of those per second. Times the step time/step size | ||
3002 | |||
3003 | fps = (timeStep / ODE_STEPSIZE) * 1000; | ||
3004 | // HACK: Using a time dilation of 1.0 to debug rubberbanding issues | ||
3005 | //m_timeDilation = Math.Min((step_time / ODE_STEPSIZE) / (0.09375f / ODE_STEPSIZE), 1.0f); | ||
3006 | |||
3007 | while (timeLeft > 0.0f) | ||
3008 | { | 1725 | { |
3009 | try | 1726 | ChangesQueue.Clear(); |
3010 | { | 1727 | return 0; |
3011 | if (CollectStats) | 1728 | } |
3012 | tempTick = Util.EnvironmentTickCount(); | ||
3013 | |||
3014 | lock (_taintedActors) | ||
3015 | { | ||
3016 | foreach (OdeCharacter character in _taintedActors) | ||
3017 | character.ProcessTaints(); | ||
3018 | 1729 | ||
3019 | _taintedActors.Clear(); | 1730 | ODEchangeitem item; |
3020 | } | ||
3021 | 1731 | ||
3022 | if (CollectStats) | 1732 | if (ChangesQueue.Count > 0) |
3023 | { | 1733 | { |
3024 | tempTick2 = Util.EnvironmentTickCount(); | 1734 | int ttmpstart = Util.EnvironmentTickCount(); |
3025 | m_stats[ODEAvatarTaintMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); | 1735 | int ttmp; |
3026 | tempTick = tempTick2; | ||
3027 | } | ||
3028 | 1736 | ||
3029 | lock (_taintedPrims) | 1737 | while (ChangesQueue.Dequeue(out item)) |
1738 | { | ||
1739 | if (item.actor != null) | ||
3030 | { | 1740 | { |
3031 | foreach (OdePrim prim in _taintedPrims) | 1741 | try |
3032 | { | 1742 | { |
3033 | if (prim.m_taintremove) | 1743 | if (item.actor is OdeCharacter) |
3034 | { | 1744 | ((OdeCharacter)item.actor).DoAChange(item.what, item.arg); |
3035 | // Console.WriteLine("Simulate calls RemovePrimThreadLocked for {0}", prim.Name); | 1745 | else if (((OdePrim)item.actor).DoAChange(item.what, item.arg)) |
3036 | RemovePrimThreadLocked(prim); | 1746 | RemovePrimThreadLocked((OdePrim)item.actor); |
3037 | } | 1747 | } |
3038 | else | 1748 | catch |
3039 | { | 1749 | { |
3040 | // Console.WriteLine("Simulate calls ProcessTaints for {0}", prim.Name); | 1750 | m_log.WarnFormat("[PHYSICS]: doChange failed for a actor {0} {1}", |
3041 | prim.ProcessTaints(); | 1751 | item.actor.Name, item.what.ToString()); |
3042 | } | ||
3043 | |||
3044 | prim.m_collisionscore = 0; | ||
3045 | |||
3046 | // This loop can block up the Heartbeat for a very long time on large regions. | ||
3047 | // We need to let the Watchdog know that the Heartbeat is not dead | ||
3048 | // NOTE: This is currently commented out, but if things like OAR loading are | ||
3049 | // timing the heartbeat out we will need to uncomment it | ||
3050 | //Watchdog.UpdateThread(); | ||
3051 | } | 1752 | } |
3052 | |||
3053 | if (SupportsNINJAJoints) | ||
3054 | SimulatePendingNINJAJoints(); | ||
3055 | |||
3056 | _taintedPrims.Clear(); | ||
3057 | } | 1753 | } |
1754 | ttmp = Util.EnvironmentTickCountSubtract(ttmpstart); | ||
1755 | if (ttmp > 20) | ||
1756 | break; | ||
1757 | } | ||
1758 | } | ||
1759 | |||
1760 | d.WorldSetQuickStepNumIterations(world, curphysiteractions); | ||
3058 | 1761 | ||
3059 | if (CollectStats) | 1762 | while (step_time > HalfOdeStep && nodeframes < 10) //limit number of steps so we don't say here for ever |
3060 | { | 1763 | { |
3061 | tempTick2 = Util.EnvironmentTickCount(); | 1764 | try |
3062 | m_stats[ODEPrimTaintMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); | 1765 | { |
3063 | tempTick = tempTick2; | 1766 | // clear pointer/counter to contacts to pass into joints |
3064 | } | 1767 | m_global_contactcount = 0; |
3065 | 1768 | ||
3066 | // Move characters | ||
3067 | foreach (OdeCharacter actor in _characters) | ||
3068 | actor.Move(defects); | ||
3069 | 1769 | ||
3070 | if (defects.Count != 0) | 1770 | // Move characters |
1771 | lock (_characters) | ||
3071 | { | 1772 | { |
3072 | foreach (OdeCharacter actor in defects) | 1773 | List<OdeCharacter> defects = new List<OdeCharacter>(); |
1774 | foreach (OdeCharacter actor in _characters) | ||
3073 | { | 1775 | { |
3074 | m_log.ErrorFormat( | 1776 | if (actor != null) |
3075 | "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2} due to defect found when moving", | 1777 | actor.Move(ODE_STEPSIZE, defects); |
3076 | actor.Name, actor.LocalID, Name); | 1778 | } |
3077 | 1779 | if (defects.Count != 0) | |
3078 | RemoveCharacter(actor); | 1780 | { |
3079 | actor.DestroyOdeStructures(); | 1781 | foreach (OdeCharacter defect in defects) |
1782 | { | ||
1783 | RemoveCharacter(defect); | ||
1784 | } | ||
1785 | defects.Clear(); | ||
3080 | } | 1786 | } |
3081 | |||
3082 | defects.Clear(); | ||
3083 | } | ||
3084 | |||
3085 | if (CollectStats) | ||
3086 | { | ||
3087 | tempTick2 = Util.EnvironmentTickCount(); | ||
3088 | m_stats[ODEAvatarForcesFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); | ||
3089 | tempTick = tempTick2; | ||
3090 | } | 1787 | } |
3091 | 1788 | ||
3092 | // Move other active objects | 1789 | // Move other active objects |
3093 | foreach (OdePrim prim in _activeprims) | 1790 | lock (_activegroups) |
3094 | { | ||
3095 | prim.m_collisionscore = 0; | ||
3096 | prim.Move(timeStep); | ||
3097 | } | ||
3098 | |||
3099 | if (CollectStats) | ||
3100 | { | 1791 | { |
3101 | tempTick2 = Util.EnvironmentTickCount(); | 1792 | foreach (OdePrim aprim in _activegroups) |
3102 | m_stats[ODEPrimForcesFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); | 1793 | { |
3103 | tempTick = tempTick2; | 1794 | aprim.Move(); |
1795 | } | ||
3104 | } | 1796 | } |
3105 | 1797 | ||
3106 | //if ((framecount % m_randomizeWater) == 0) | 1798 | //if ((framecount % m_randomizeWater) == 0) |
3107 | // randomizeWater(waterlevel); | 1799 | // randomizeWater(waterlevel); |
3108 | 1800 | ||
3109 | //int RayCastTimeMS = m_rayCastManager.ProcessQueuedRequests(); | ||
3110 | m_rayCastManager.ProcessQueuedRequests(); | 1801 | m_rayCastManager.ProcessQueuedRequests(); |
3111 | 1802 | ||
3112 | if (CollectStats) | ||
3113 | { | ||
3114 | tempTick2 = Util.EnvironmentTickCount(); | ||
3115 | m_stats[ODERaycastingFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); | ||
3116 | tempTick = tempTick2; | ||
3117 | } | ||
3118 | |||
3119 | collision_optimized(); | 1803 | collision_optimized(); |
3120 | 1804 | ||
3121 | if (CollectStats) | 1805 | foreach (PhysicsActor obj in _collisionEventPrim) |
3122 | { | ||
3123 | tempTick2 = Util.EnvironmentTickCount(); | ||
3124 | m_stats[ODEOtherCollisionFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); | ||
3125 | tempTick = tempTick2; | ||
3126 | } | ||
3127 | |||
3128 | foreach (PhysicsActor obj in m_collisionEventActors.Values) | ||
3129 | { | 1806 | { |
3130 | // m_log.DebugFormat("[PHYSICS]: Assessing {0} {1} for collision events", obj.SOPName, obj.LocalID); | 1807 | if (obj == null) |
1808 | continue; | ||
3131 | 1809 | ||
3132 | switch ((ActorTypes)obj.PhysicsActorType) | 1810 | switch ((ActorTypes)obj.PhysicsActorType) |
3133 | { | 1811 | { |
3134 | case ActorTypes.Agent: | 1812 | case ActorTypes.Agent: |
3135 | OdeCharacter cobj = (OdeCharacter)obj; | 1813 | OdeCharacter cobj = (OdeCharacter)obj; |
3136 | cobj.AddCollisionFrameTime(100); | 1814 | cobj.AddCollisionFrameTime((int)(odetimestepMS)); |
3137 | cobj.SendCollisions(); | 1815 | cobj.SendCollisions(); |
3138 | break; | 1816 | break; |
3139 | 1817 | ||
3140 | case ActorTypes.Prim: | 1818 | case ActorTypes.Prim: |
3141 | OdePrim pobj = (OdePrim)obj; | 1819 | OdePrim pobj = (OdePrim)obj; |
3142 | pobj.SendCollisions(); | 1820 | if (pobj.Body == IntPtr.Zero || (d.BodyIsEnabled(pobj.Body) && !pobj.m_outbounds)) |
1821 | if (!pobj.m_outbounds) | ||
1822 | { | ||
1823 | pobj.AddCollisionFrameTime((int)(odetimestepMS)); | ||
1824 | pobj.SendCollisions(); | ||
1825 | } | ||
3143 | break; | 1826 | break; |
3144 | } | 1827 | } |
3145 | } | 1828 | } |
3146 | 1829 | ||
3147 | // if (m_global_contactcount > 0) | 1830 | foreach (PhysicsActor obj in _collisionEventPrimRemove) |
3148 | // m_log.DebugFormat( | 1831 | _collisionEventPrim.Remove(obj); |
3149 | // "[PHYSICS]: Collision contacts to process this frame = {0}", m_global_contactcount); | ||
3150 | |||
3151 | m_global_contactcount = 0; | ||
3152 | 1832 | ||
3153 | if (CollectStats) | 1833 | _collisionEventPrimRemove.Clear(); |
3154 | { | ||
3155 | tempTick2 = Util.EnvironmentTickCount(); | ||
3156 | m_stats[ODECollisionNotificationFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); | ||
3157 | tempTick = tempTick2; | ||
3158 | } | ||
3159 | 1834 | ||
1835 | // do a ode simulation step | ||
3160 | d.WorldQuickStep(world, ODE_STEPSIZE); | 1836 | d.WorldQuickStep(world, ODE_STEPSIZE); |
1837 | d.JointGroupEmpty(contactgroup); | ||
3161 | 1838 | ||
3162 | if (CollectStats) | 1839 | // update managed ideia of physical data and do updates to core |
3163 | m_stats[ODENativeStepFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick); | 1840 | /* |
1841 | lock (_characters) | ||
1842 | { | ||
1843 | foreach (OdeCharacter actor in _characters) | ||
1844 | { | ||
1845 | if (actor != null) | ||
1846 | { | ||
1847 | if (actor.bad) | ||
1848 | m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid); | ||
3164 | 1849 | ||
3165 | d.JointGroupEmpty(contactgroup); | 1850 | actor.UpdatePositionAndVelocity(); |
1851 | } | ||
1852 | } | ||
1853 | } | ||
1854 | */ | ||
1855 | |||
1856 | lock (_activegroups) | ||
1857 | { | ||
1858 | { | ||
1859 | foreach (OdePrim actor in _activegroups) | ||
1860 | { | ||
1861 | if (actor.IsPhysical) | ||
1862 | { | ||
1863 | actor.UpdatePositionAndVelocity(); | ||
1864 | } | ||
1865 | } | ||
1866 | } | ||
1867 | } | ||
3166 | } | 1868 | } |
3167 | catch (Exception e) | 1869 | catch (Exception e) |
3168 | { | 1870 | { |
3169 | m_log.ErrorFormat("[ODE SCENE]: {0}, {1}, {2}", e.Message, e.TargetSite, e); | 1871 | m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e); |
1872 | // ode.dunlock(world); | ||
3170 | } | 1873 | } |
3171 | 1874 | ||
3172 | timeLeft -= ODE_STEPSIZE; | ||
3173 | } | ||
3174 | 1875 | ||
3175 | if (CollectStats) | 1876 | step_time -= ODE_STEPSIZE; |
3176 | tempTick = Util.EnvironmentTickCount(); | 1877 | nodeframes++; |
3177 | |||
3178 | foreach (OdeCharacter actor in _characters) | ||
3179 | { | ||
3180 | if (actor.bad) | ||
3181 | m_log.ErrorFormat("[ODE SCENE]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid); | ||
3182 | |||
3183 | actor.UpdatePositionAndVelocity(defects); | ||
3184 | } | 1878 | } |
3185 | 1879 | ||
3186 | if (defects.Count != 0) | 1880 | lock (_badCharacter) |
3187 | { | 1881 | { |
3188 | foreach (OdeCharacter actor in defects) | 1882 | if (_badCharacter.Count > 0) |
3189 | { | 1883 | { |
3190 | m_log.ErrorFormat( | 1884 | foreach (OdeCharacter chr in _badCharacter) |
3191 | "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2} due to defect found when updating position and velocity", | 1885 | { |
3192 | actor.Name, actor.LocalID, Name); | 1886 | RemoveCharacter(chr); |
1887 | } | ||
3193 | 1888 | ||
3194 | RemoveCharacter(actor); | 1889 | _badCharacter.Clear(); |
3195 | actor.DestroyOdeStructures(); | ||
3196 | } | 1890 | } |
3197 | |||
3198 | defects.Clear(); | ||
3199 | } | 1891 | } |
3200 | 1892 | ||
3201 | if (CollectStats) | 1893 | timedif = now - m_lastMeshExpire; |
1894 | |||
1895 | if (timedif.Seconds > 10) | ||
3202 | { | 1896 | { |
3203 | tempTick2 = Util.EnvironmentTickCount(); | 1897 | mesher.ExpireReleaseMeshs(); |
3204 | m_stats[ODEAvatarUpdateFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); | 1898 | m_lastMeshExpire = now; |
3205 | tempTick = tempTick2; | ||
3206 | } | 1899 | } |
3207 | 1900 | ||
3208 | //if (timeStep < 0.2f) | 1901 | // information block running in debug only |
1902 | /* | ||
1903 | int ntopactivegeoms = d.SpaceGetNumGeoms(ActiveSpace); | ||
1904 | int ntopstaticgeoms = d.SpaceGetNumGeoms(StaticSpace); | ||
1905 | int ngroundgeoms = d.SpaceGetNumGeoms(GroundSpace); | ||
1906 | |||
1907 | int nactivegeoms = 0; | ||
1908 | int nactivespaces = 0; | ||
3209 | 1909 | ||
3210 | foreach (OdePrim prim in _activeprims) | 1910 | int nstaticgeoms = 0; |
1911 | int nstaticspaces = 0; | ||
1912 | IntPtr sp; | ||
1913 | |||
1914 | for (int i = 0; i < ntopactivegeoms; i++) | ||
3211 | { | 1915 | { |
3212 | if (prim.IsPhysical && (d.BodyIsEnabled(prim.Body) || !prim._zeroFlag)) | 1916 | sp = d.SpaceGetGeom(ActiveSpace, i); |
1917 | if (d.GeomIsSpace(sp)) | ||
3213 | { | 1918 | { |
3214 | prim.UpdatePositionAndVelocity(); | 1919 | nactivespaces++; |
3215 | 1920 | nactivegeoms += d.SpaceGetNumGeoms(sp); | |
3216 | if (SupportsNINJAJoints) | ||
3217 | SimulateActorPendingJoints(prim); | ||
3218 | } | 1921 | } |
1922 | else | ||
1923 | nactivegeoms++; | ||
3219 | } | 1924 | } |
3220 | 1925 | ||
3221 | if (CollectStats) | 1926 | for (int i = 0; i < ntopstaticgeoms; i++) |
3222 | m_stats[ODEPrimUpdateFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick); | 1927 | { |
1928 | sp = d.SpaceGetGeom(StaticSpace, i); | ||
1929 | if (d.GeomIsSpace(sp)) | ||
1930 | { | ||
1931 | nstaticspaces++; | ||
1932 | nstaticgeoms += d.SpaceGetNumGeoms(sp); | ||
1933 | } | ||
1934 | else | ||
1935 | nstaticgeoms++; | ||
1936 | } | ||
3223 | 1937 | ||
3224 | //DumpJointInfo(); | 1938 | int ntopgeoms = d.SpaceGetNumGeoms(TopSpace); |
3225 | 1939 | ||
1940 | int totgeoms = nstaticgeoms + nactivegeoms + ngroundgeoms + 1; // one ray | ||
1941 | int nbodies = d.NTotalBodies; | ||
1942 | int ngeoms = d.NTotalGeoms; | ||
1943 | */ | ||
3226 | // Finished with all sim stepping. If requested, dump world state to file for debugging. | 1944 | // Finished with all sim stepping. If requested, dump world state to file for debugging. |
3227 | // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed? | 1945 | // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed? |
3228 | // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots? | 1946 | // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots? |
@@ -3241,256 +1959,26 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
3241 | 1959 | ||
3242 | d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix); | 1960 | d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix); |
3243 | } | 1961 | } |
3244 | 1962 | ||
3245 | latertickcount = Util.EnvironmentTickCountSubtract(tickCountFrameRun); | 1963 | // think time dilation as to do with dinamic step size that we dont' have |
3246 | 1964 | // even so tell something to world | |
3247 | // OpenSimulator above does 10 fps. 10 fps = means that the main thread loop and physics | 1965 | if (nodeframes < 10) // we did the requested loops |
3248 | // has a max of 100 ms to run theoretically. | ||
3249 | // If the main loop stalls, it calls Simulate later which makes the tick count ms larger. | ||
3250 | // If Physics stalls, it takes longer which makes the tick count ms larger. | ||
3251 | |||
3252 | if (latertickcount < 100) | ||
3253 | { | ||
3254 | m_timeDilation = 1.0f; | 1966 | m_timeDilation = 1.0f; |
3255 | } | 1967 | else if (step_time > 0) |
3256 | else | ||
3257 | { | 1968 | { |
3258 | m_timeDilation = 100f / latertickcount; | 1969 | m_timeDilation = timeStep / step_time; |
3259 | //m_timeDilation = Math.Min((Math.Max(100 - (Util.EnvironmentTickCount() - tickCountFrameRun), 1) / 100f), 1.0f); | 1970 | if (m_timeDilation > 1) |
1971 | m_timeDilation = 1; | ||
1972 | if (step_time > m_SkipFramesAtms) | ||
1973 | step_time = 0; | ||
3260 | } | 1974 | } |
3261 | |||
3262 | tickCountFrameRun = Util.EnvironmentTickCount(); | ||
3263 | |||
3264 | if (CollectStats) | ||
3265 | m_stats[ODETotalFrameMsStatName] += Util.EnvironmentTickCountSubtract(startFrameTick); | ||
3266 | } | 1975 | } |
3267 | 1976 | ||
3268 | return fps; | 1977 | // return nodeframes * ODE_STEPSIZE; // return real simulated time |
1978 | return 1000 * nodeframes; // return steps for now * 1000 to keep core happy | ||
3269 | } | 1979 | } |
3270 | 1980 | ||
3271 | /// <summary> | 1981 | /// <summary> |
3272 | /// Simulate pending NINJA joints. | ||
3273 | /// </summary> | ||
3274 | /// <remarks> | ||
3275 | /// Called by the main Simulate() loop if NINJA joints are active. Should not be called from anywhere else. | ||
3276 | /// </remarks> | ||
3277 | private void SimulatePendingNINJAJoints() | ||
3278 | { | ||
3279 | // Create pending joints, if possible | ||
3280 | |||
3281 | // joints can only be processed after ALL bodies are processed (and exist in ODE), since creating | ||
3282 | // a joint requires specifying the body id of both involved bodies | ||
3283 | if (pendingJoints.Count > 0) | ||
3284 | { | ||
3285 | List<PhysicsJoint> successfullyProcessedPendingJoints = new List<PhysicsJoint>(); | ||
3286 | //DoJointErrorMessage(joints_connecting_actor, "taint: " + pendingJoints.Count + " pending joints"); | ||
3287 | foreach (PhysicsJoint joint in pendingJoints) | ||
3288 | { | ||
3289 | //DoJointErrorMessage(joint, "taint: time to create joint with parms: " + joint.RawParams); | ||
3290 | string[] jointParams = joint.RawParams.Split(" ".ToCharArray(), System.StringSplitOptions.RemoveEmptyEntries); | ||
3291 | List<IntPtr> jointBodies = new List<IntPtr>(); | ||
3292 | bool allJointBodiesAreReady = true; | ||
3293 | foreach (string jointParam in jointParams) | ||
3294 | { | ||
3295 | if (jointParam == "NULL") | ||
3296 | { | ||
3297 | //DoJointErrorMessage(joint, "attaching NULL joint to world"); | ||
3298 | jointBodies.Add(IntPtr.Zero); | ||
3299 | } | ||
3300 | else | ||
3301 | { | ||
3302 | //DoJointErrorMessage(joint, "looking for prim name: " + jointParam); | ||
3303 | bool foundPrim = false; | ||
3304 | lock (_prims) | ||
3305 | { | ||
3306 | foreach (OdePrim prim in _prims) // FIXME: inefficient | ||
3307 | { | ||
3308 | if (prim.SOPName == jointParam) | ||
3309 | { | ||
3310 | //DoJointErrorMessage(joint, "found for prim name: " + jointParam); | ||
3311 | if (prim.IsPhysical && prim.Body != IntPtr.Zero) | ||
3312 | { | ||
3313 | jointBodies.Add(prim.Body); | ||
3314 | foundPrim = true; | ||
3315 | break; | ||
3316 | } | ||
3317 | else | ||
3318 | { | ||
3319 | DoJointErrorMessage(joint, "prim name " + jointParam + | ||
3320 | " exists but is not (yet) physical; deferring joint creation. " + | ||
3321 | "IsPhysical property is " + prim.IsPhysical + | ||
3322 | " and body is " + prim.Body); | ||
3323 | foundPrim = false; | ||
3324 | break; | ||
3325 | } | ||
3326 | } | ||
3327 | } | ||
3328 | } | ||
3329 | if (foundPrim) | ||
3330 | { | ||
3331 | // all is fine | ||
3332 | } | ||
3333 | else | ||
3334 | { | ||
3335 | allJointBodiesAreReady = false; | ||
3336 | break; | ||
3337 | } | ||
3338 | } | ||
3339 | } | ||
3340 | |||
3341 | if (allJointBodiesAreReady) | ||
3342 | { | ||
3343 | //DoJointErrorMessage(joint, "allJointBodiesAreReady for " + joint.ObjectNameInScene + " with parms " + joint.RawParams); | ||
3344 | if (jointBodies[0] == jointBodies[1]) | ||
3345 | { | ||
3346 | DoJointErrorMessage(joint, "ERROR: joint cannot be created; the joint bodies are the same, body1==body2. Raw body is " + jointBodies[0] + ". raw parms: " + joint.RawParams); | ||
3347 | } | ||
3348 | else | ||
3349 | { | ||
3350 | switch (joint.Type) | ||
3351 | { | ||
3352 | case PhysicsJointType.Ball: | ||
3353 | { | ||
3354 | IntPtr odeJoint; | ||
3355 | //DoJointErrorMessage(joint, "ODE creating ball joint "); | ||
3356 | odeJoint = d.JointCreateBall(world, IntPtr.Zero); | ||
3357 | //DoJointErrorMessage(joint, "ODE attaching ball joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]); | ||
3358 | d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]); | ||
3359 | //DoJointErrorMessage(joint, "ODE setting ball anchor: " + odeJoint + " to vec:" + joint.Position); | ||
3360 | d.JointSetBallAnchor(odeJoint, | ||
3361 | joint.Position.X, | ||
3362 | joint.Position.Y, | ||
3363 | joint.Position.Z); | ||
3364 | //DoJointErrorMessage(joint, "ODE joint setting OK"); | ||
3365 | //DoJointErrorMessage(joint, "The ball joint's bodies are here: b0: "); | ||
3366 | //DoJointErrorMessage(joint, "" + (jointBodies[0] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[0]) : "fixed environment")); | ||
3367 | //DoJointErrorMessage(joint, "The ball joint's bodies are here: b1: "); | ||
3368 | //DoJointErrorMessage(joint, "" + (jointBodies[1] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[1]) : "fixed environment")); | ||
3369 | |||
3370 | if (joint is OdePhysicsJoint) | ||
3371 | { | ||
3372 | ((OdePhysicsJoint)joint).jointID = odeJoint; | ||
3373 | } | ||
3374 | else | ||
3375 | { | ||
3376 | DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!"); | ||
3377 | } | ||
3378 | } | ||
3379 | break; | ||
3380 | case PhysicsJointType.Hinge: | ||
3381 | { | ||
3382 | IntPtr odeJoint; | ||
3383 | //DoJointErrorMessage(joint, "ODE creating hinge joint "); | ||
3384 | odeJoint = d.JointCreateHinge(world, IntPtr.Zero); | ||
3385 | //DoJointErrorMessage(joint, "ODE attaching hinge joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]); | ||
3386 | d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]); | ||
3387 | //DoJointErrorMessage(joint, "ODE setting hinge anchor: " + odeJoint + " to vec:" + joint.Position); | ||
3388 | d.JointSetHingeAnchor(odeJoint, | ||
3389 | joint.Position.X, | ||
3390 | joint.Position.Y, | ||
3391 | joint.Position.Z); | ||
3392 | // We use the orientation of the x-axis of the joint's coordinate frame | ||
3393 | // as the axis for the hinge. | ||
3394 | |||
3395 | // Therefore, we must get the joint's coordinate frame based on the | ||
3396 | // joint.Rotation field, which originates from the orientation of the | ||
3397 | // joint's proxy object in the scene. | ||
3398 | |||
3399 | // The joint's coordinate frame is defined as the transformation matrix | ||
3400 | // that converts a vector from joint-local coordinates into world coordinates. | ||
3401 | // World coordinates are defined as the XYZ coordinate system of the sim, | ||
3402 | // as shown in the top status-bar of the viewer. | ||
3403 | |||
3404 | // Once we have the joint's coordinate frame, we extract its X axis (AtAxis) | ||
3405 | // and use that as the hinge axis. | ||
3406 | |||
3407 | //joint.Rotation.Normalize(); | ||
3408 | Matrix4 proxyFrame = Matrix4.CreateFromQuaternion(joint.Rotation); | ||
3409 | |||
3410 | // Now extract the X axis of the joint's coordinate frame. | ||
3411 | |||
3412 | // Do not try to use proxyFrame.AtAxis or you will become mired in the | ||
3413 | // tar pit of transposed, inverted, and generally messed-up orientations. | ||
3414 | // (In other words, Matrix4.AtAxis() is borked.) | ||
3415 | // Vector3 jointAxis = proxyFrame.AtAxis; <--- this path leadeth to madness | ||
3416 | |||
3417 | // Instead, compute the X axis of the coordinate frame by transforming | ||
3418 | // the (1,0,0) vector. At least that works. | ||
3419 | |||
3420 | //m_log.Debug("PHY: making axis: complete matrix is " + proxyFrame); | ||
3421 | Vector3 jointAxis = Vector3.Transform(Vector3.UnitX, proxyFrame); | ||
3422 | //m_log.Debug("PHY: making axis: hinge joint axis is " + jointAxis); | ||
3423 | //DoJointErrorMessage(joint, "ODE setting hinge axis: " + odeJoint + " to vec:" + jointAxis); | ||
3424 | d.JointSetHingeAxis(odeJoint, | ||
3425 | jointAxis.X, | ||
3426 | jointAxis.Y, | ||
3427 | jointAxis.Z); | ||
3428 | //d.JointSetHingeParam(odeJoint, (int)dParam.CFM, 0.1f); | ||
3429 | if (joint is OdePhysicsJoint) | ||
3430 | { | ||
3431 | ((OdePhysicsJoint)joint).jointID = odeJoint; | ||
3432 | } | ||
3433 | else | ||
3434 | { | ||
3435 | DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!"); | ||
3436 | } | ||
3437 | } | ||
3438 | break; | ||
3439 | } | ||
3440 | successfullyProcessedPendingJoints.Add(joint); | ||
3441 | } | ||
3442 | } | ||
3443 | else | ||
3444 | { | ||
3445 | DoJointErrorMessage(joint, "joint could not yet be created; still pending"); | ||
3446 | } | ||
3447 | } | ||
3448 | |||
3449 | foreach (PhysicsJoint successfullyProcessedJoint in successfullyProcessedPendingJoints) | ||
3450 | { | ||
3451 | //DoJointErrorMessage(successfullyProcessedJoint, "finalizing succesfully procsssed joint " + successfullyProcessedJoint.ObjectNameInScene + " parms " + successfullyProcessedJoint.RawParams); | ||
3452 | //DoJointErrorMessage(successfullyProcessedJoint, "removing from pending"); | ||
3453 | InternalRemovePendingJoint(successfullyProcessedJoint); | ||
3454 | //DoJointErrorMessage(successfullyProcessedJoint, "adding to active"); | ||
3455 | InternalAddActiveJoint(successfullyProcessedJoint); | ||
3456 | //DoJointErrorMessage(successfullyProcessedJoint, "done"); | ||
3457 | } | ||
3458 | } | ||
3459 | } | ||
3460 | |||
3461 | /// <summary> | ||
3462 | /// Simulate the joint proxies of a NINJA actor. | ||
3463 | /// </summary> | ||
3464 | /// <remarks> | ||
3465 | /// Called as part of the Simulate() loop if NINJA physics is active. Must only be called from there. | ||
3466 | /// </remarks> | ||
3467 | /// <param name="actor"></param> | ||
3468 | private void SimulateActorPendingJoints(OdePrim actor) | ||
3469 | { | ||
3470 | // If an actor moved, move its joint proxy objects as well. | ||
3471 | // There seems to be an event PhysicsActor.OnPositionUpdate that could be used | ||
3472 | // for this purpose but it is never called! So we just do the joint | ||
3473 | // movement code here. | ||
3474 | |||
3475 | if (actor.SOPName != null && | ||
3476 | joints_connecting_actor.ContainsKey(actor.SOPName) && | ||
3477 | joints_connecting_actor[actor.SOPName] != null && | ||
3478 | joints_connecting_actor[actor.SOPName].Count > 0) | ||
3479 | { | ||
3480 | foreach (PhysicsJoint affectedJoint in joints_connecting_actor[actor.SOPName]) | ||
3481 | { | ||
3482 | if (affectedJoint.IsInPhysicsEngine) | ||
3483 | { | ||
3484 | DoJointMoved(affectedJoint); | ||
3485 | } | ||
3486 | else | ||
3487 | { | ||
3488 | DoJointErrorMessage(affectedJoint, "a body connected to a joint was moved, but the joint doesn't exist yet! this will lead to joint error. joint was: " + affectedJoint.ObjectNameInScene + " parms:" + affectedJoint.RawParams); | ||
3489 | } | ||
3490 | } | ||
3491 | } | ||
3492 | } | ||
3493 | |||
3494 | public override void GetResults() | 1982 | public override void GetResults() |
3495 | { | 1983 | { |
3496 | } | 1984 | } |
@@ -3498,275 +1986,141 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
3498 | public override bool IsThreaded | 1986 | public override bool IsThreaded |
3499 | { | 1987 | { |
3500 | // for now we won't be multithreaded | 1988 | // for now we won't be multithreaded |
3501 | get { return false; } | 1989 | get { return (false); } |
3502 | } | 1990 | } |
3503 | 1991 | ||
3504 | #region ODE Specific Terrain Fixes | 1992 | public float GetTerrainHeightAtXY(float x, float y) |
3505 | private float[] ResizeTerrain512NearestNeighbour(float[] heightMap) | ||
3506 | { | 1993 | { |
3507 | float[] returnarr = new float[262144]; | ||
3508 | float[,] resultarr = new float[(int)WorldExtents.X, (int)WorldExtents.Y]; | ||
3509 | 1994 | ||
3510 | // Filling out the array into its multi-dimensional components | ||
3511 | for (int y = 0; y < WorldExtents.Y; y++) | ||
3512 | { | ||
3513 | for (int x = 0; x < WorldExtents.X; x++) | ||
3514 | { | ||
3515 | resultarr[y, x] = heightMap[y * (int)WorldExtents.Y + x]; | ||
3516 | } | ||
3517 | } | ||
3518 | 1995 | ||
3519 | // Resize using Nearest Neighbour | 1996 | int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; |
3520 | 1997 | int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; | |
3521 | // This particular way is quick but it only works on a multiple of the original | ||
3522 | |||
3523 | // The idea behind this method can be described with the following diagrams | ||
3524 | // second pass and third pass happen in the same loop really.. just separated | ||
3525 | // them to show what this does. | ||
3526 | |||
3527 | // First Pass | ||
3528 | // ResultArr: | ||
3529 | // 1,1,1,1,1,1 | ||
3530 | // 1,1,1,1,1,1 | ||
3531 | // 1,1,1,1,1,1 | ||
3532 | // 1,1,1,1,1,1 | ||
3533 | // 1,1,1,1,1,1 | ||
3534 | // 1,1,1,1,1,1 | ||
3535 | |||
3536 | // Second Pass | ||
3537 | // ResultArr2: | ||
3538 | // 1,,1,,1,,1,,1,,1, | ||
3539 | // ,,,,,,,,,, | ||
3540 | // 1,,1,,1,,1,,1,,1, | ||
3541 | // ,,,,,,,,,, | ||
3542 | // 1,,1,,1,,1,,1,,1, | ||
3543 | // ,,,,,,,,,, | ||
3544 | // 1,,1,,1,,1,,1,,1, | ||
3545 | // ,,,,,,,,,, | ||
3546 | // 1,,1,,1,,1,,1,,1, | ||
3547 | // ,,,,,,,,,, | ||
3548 | // 1,,1,,1,,1,,1,,1, | ||
3549 | |||
3550 | // Third pass fills in the blanks | ||
3551 | // ResultArr2: | ||
3552 | // 1,1,1,1,1,1,1,1,1,1,1,1 | ||
3553 | // 1,1,1,1,1,1,1,1,1,1,1,1 | ||
3554 | // 1,1,1,1,1,1,1,1,1,1,1,1 | ||
3555 | // 1,1,1,1,1,1,1,1,1,1,1,1 | ||
3556 | // 1,1,1,1,1,1,1,1,1,1,1,1 | ||
3557 | // 1,1,1,1,1,1,1,1,1,1,1,1 | ||
3558 | // 1,1,1,1,1,1,1,1,1,1,1,1 | ||
3559 | // 1,1,1,1,1,1,1,1,1,1,1,1 | ||
3560 | // 1,1,1,1,1,1,1,1,1,1,1,1 | ||
3561 | // 1,1,1,1,1,1,1,1,1,1,1,1 | ||
3562 | // 1,1,1,1,1,1,1,1,1,1,1,1 | ||
3563 | |||
3564 | // X,Y = . | ||
3565 | // X+1,y = ^ | ||
3566 | // X,Y+1 = * | ||
3567 | // X+1,Y+1 = # | ||
3568 | |||
3569 | // Filling in like this; | ||
3570 | // .* | ||
3571 | // ^# | ||
3572 | // 1st . | ||
3573 | // 2nd * | ||
3574 | // 3rd ^ | ||
3575 | // 4th # | ||
3576 | // on single loop. | ||
3577 | |||
3578 | float[,] resultarr2 = new float[512, 512]; | ||
3579 | for (int y = 0; y < WorldExtents.Y; y++) | ||
3580 | { | ||
3581 | for (int x = 0; x < WorldExtents.X; x++) | ||
3582 | { | ||
3583 | resultarr2[y * 2, x * 2] = resultarr[y, x]; | ||
3584 | 1998 | ||
3585 | if (y < WorldExtents.Y) | ||
3586 | { | ||
3587 | resultarr2[(y * 2) + 1, x * 2] = resultarr[y, x]; | ||
3588 | } | ||
3589 | if (x < WorldExtents.X) | ||
3590 | { | ||
3591 | resultarr2[y * 2, (x * 2) + 1] = resultarr[y, x]; | ||
3592 | } | ||
3593 | if (x < WorldExtents.X && y < WorldExtents.Y) | ||
3594 | { | ||
3595 | resultarr2[(y * 2) + 1, (x * 2) + 1] = resultarr[y, x]; | ||
3596 | } | ||
3597 | } | ||
3598 | } | ||
3599 | 1999 | ||
3600 | //Flatten out the array | 2000 | IntPtr heightFieldGeom = IntPtr.Zero; |
3601 | int i = 0; | ||
3602 | for (int y = 0; y < 512; y++) | ||
3603 | { | ||
3604 | for (int x = 0; x < 512; x++) | ||
3605 | { | ||
3606 | if (resultarr2[y, x] <= 0) | ||
3607 | returnarr[i] = 0.0000001f; | ||
3608 | else | ||
3609 | returnarr[i] = resultarr2[y, x]; | ||
3610 | 2001 | ||
3611 | i++; | 2002 | // get region map |
3612 | } | 2003 | if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom)) |
3613 | } | 2004 | return 0f; |
3614 | 2005 | ||
3615 | return returnarr; | 2006 | if (heightFieldGeom == IntPtr.Zero) |
3616 | } | 2007 | return 0f; |
3617 | 2008 | ||
3618 | private float[] ResizeTerrain512Interpolation(float[] heightMap) | 2009 | if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom)) |
3619 | { | 2010 | return 0f; |
3620 | float[] returnarr = new float[262144]; | 2011 | |
3621 | float[,] resultarr = new float[512,512]; | 2012 | // TerrainHeightField for ODE as offset 1m |
2013 | x += 1f - offsetX; | ||
2014 | y += 1f - offsetY; | ||
2015 | |||
2016 | // make position fit into array | ||
2017 | if (x < 0) | ||
2018 | x = 0; | ||
2019 | if (y < 0) | ||
2020 | y = 0; | ||
3622 | 2021 | ||
3623 | // Filling out the array into its multi-dimensional components | 2022 | // integer indexs |
3624 | for (int y = 0; y < 256; y++) | 2023 | int ix; |
2024 | int iy; | ||
2025 | // interpolators offset | ||
2026 | float dx; | ||
2027 | float dy; | ||
2028 | |||
2029 | int regsize = (int)Constants.RegionSize + 3; // map size see setterrain number of samples | ||
2030 | |||
2031 | if (OdeUbitLib) | ||
3625 | { | 2032 | { |
3626 | for (int x = 0; x < 256; x++) | 2033 | if (x < regsize - 1) |
2034 | { | ||
2035 | ix = (int)x; | ||
2036 | dx = x - (float)ix; | ||
2037 | } | ||
2038 | else // out world use external height | ||
2039 | { | ||
2040 | ix = regsize - 2; | ||
2041 | dx = 0; | ||
2042 | } | ||
2043 | if (y < regsize - 1) | ||
3627 | { | 2044 | { |
3628 | resultarr[y, x] = heightMap[y * 256 + x]; | 2045 | iy = (int)y; |
2046 | dy = y - (float)iy; | ||
2047 | } | ||
2048 | else | ||
2049 | { | ||
2050 | iy = regsize - 2; | ||
2051 | dy = 0; | ||
3629 | } | 2052 | } |
3630 | } | 2053 | } |
3631 | 2054 | ||
3632 | // Resize using interpolation | 2055 | else |
3633 | |||
3634 | // This particular way is quick but it only works on a multiple of the original | ||
3635 | |||
3636 | // The idea behind this method can be described with the following diagrams | ||
3637 | // second pass and third pass happen in the same loop really.. just separated | ||
3638 | // them to show what this does. | ||
3639 | |||
3640 | // First Pass | ||
3641 | // ResultArr: | ||
3642 | // 1,1,1,1,1,1 | ||
3643 | // 1,1,1,1,1,1 | ||
3644 | // 1,1,1,1,1,1 | ||
3645 | // 1,1,1,1,1,1 | ||
3646 | // 1,1,1,1,1,1 | ||
3647 | // 1,1,1,1,1,1 | ||
3648 | |||
3649 | // Second Pass | ||
3650 | // ResultArr2: | ||
3651 | // 1,,1,,1,,1,,1,,1, | ||
3652 | // ,,,,,,,,,, | ||
3653 | // 1,,1,,1,,1,,1,,1, | ||
3654 | // ,,,,,,,,,, | ||
3655 | // 1,,1,,1,,1,,1,,1, | ||
3656 | // ,,,,,,,,,, | ||
3657 | // 1,,1,,1,,1,,1,,1, | ||
3658 | // ,,,,,,,,,, | ||
3659 | // 1,,1,,1,,1,,1,,1, | ||
3660 | // ,,,,,,,,,, | ||
3661 | // 1,,1,,1,,1,,1,,1, | ||
3662 | |||
3663 | // Third pass fills in the blanks | ||
3664 | // ResultArr2: | ||
3665 | // 1,1,1,1,1,1,1,1,1,1,1,1 | ||
3666 | // 1,1,1,1,1,1,1,1,1,1,1,1 | ||
3667 | // 1,1,1,1,1,1,1,1,1,1,1,1 | ||
3668 | // 1,1,1,1,1,1,1,1,1,1,1,1 | ||
3669 | // 1,1,1,1,1,1,1,1,1,1,1,1 | ||
3670 | // 1,1,1,1,1,1,1,1,1,1,1,1 | ||
3671 | // 1,1,1,1,1,1,1,1,1,1,1,1 | ||
3672 | // 1,1,1,1,1,1,1,1,1,1,1,1 | ||
3673 | // 1,1,1,1,1,1,1,1,1,1,1,1 | ||
3674 | // 1,1,1,1,1,1,1,1,1,1,1,1 | ||
3675 | // 1,1,1,1,1,1,1,1,1,1,1,1 | ||
3676 | |||
3677 | // X,Y = . | ||
3678 | // X+1,y = ^ | ||
3679 | // X,Y+1 = * | ||
3680 | // X+1,Y+1 = # | ||
3681 | |||
3682 | // Filling in like this; | ||
3683 | // .* | ||
3684 | // ^# | ||
3685 | // 1st . | ||
3686 | // 2nd * | ||
3687 | // 3rd ^ | ||
3688 | // 4th # | ||
3689 | // on single loop. | ||
3690 | |||
3691 | float[,] resultarr2 = new float[512,512]; | ||
3692 | for (int y = 0; y < (int)Constants.RegionSize; y++) | ||
3693 | { | 2056 | { |
3694 | for (int x = 0; x < (int)Constants.RegionSize; x++) | 2057 | // we still have square fixed size regions |
2058 | // also flip x and y because of how map is done for ODE fliped axis | ||
2059 | // so ix,iy,dx and dy are inter exchanged | ||
2060 | if (x < regsize - 1) | ||
2061 | { | ||
2062 | iy = (int)x; | ||
2063 | dy = x - (float)iy; | ||
2064 | } | ||
2065 | else // out world use external height | ||
2066 | { | ||
2067 | iy = regsize - 2; | ||
2068 | dy = 0; | ||
2069 | } | ||
2070 | if (y < regsize - 1) | ||
2071 | { | ||
2072 | ix = (int)y; | ||
2073 | dx = y - (float)ix; | ||
2074 | } | ||
2075 | else | ||
3695 | { | 2076 | { |
3696 | resultarr2[y*2, x*2] = resultarr[y, x]; | 2077 | ix = regsize - 2; |
2078 | dx = 0; | ||
2079 | } | ||
2080 | } | ||
3697 | 2081 | ||
3698 | if (y < (int)Constants.RegionSize) | 2082 | float h0; |
3699 | { | 2083 | float h1; |
3700 | if (y + 1 < (int)Constants.RegionSize) | 2084 | float h2; |
3701 | { | 2085 | |
3702 | if (x + 1 < (int)Constants.RegionSize) | 2086 | iy *= regsize; |
3703 | { | 2087 | iy += ix; // all indexes have iy + ix |
3704 | resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x] + | 2088 | |
3705 | resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4); | 2089 | float[] heights = TerrainHeightFieldHeights[heightFieldGeom]; |
3706 | } | 2090 | /* |
3707 | else | 2091 | if ((dx + dy) <= 1.0f) |
3708 | { | ||
3709 | resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x])/2); | ||
3710 | } | ||
3711 | } | ||
3712 | else | ||
3713 | { | ||
3714 | resultarr2[(y*2) + 1, x*2] = resultarr[y, x]; | ||
3715 | } | ||
3716 | } | ||
3717 | if (x < (int)Constants.RegionSize) | ||
3718 | { | ||
3719 | if (x + 1 < (int)Constants.RegionSize) | ||
3720 | { | ||
3721 | if (y + 1 < (int)Constants.RegionSize) | ||
3722 | { | ||
3723 | resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] + | ||
3724 | resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4); | ||
3725 | } | ||
3726 | else | ||
3727 | { | ||
3728 | resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y, x + 1])/2); | ||
3729 | } | ||
3730 | } | ||
3731 | else | ||
3732 | { | ||
3733 | resultarr2[y*2, (x*2) + 1] = resultarr[y, x]; | ||
3734 | } | ||
3735 | } | ||
3736 | if (x < (int)Constants.RegionSize && y < (int)Constants.RegionSize) | ||
3737 | { | ||
3738 | if ((x + 1 < (int)Constants.RegionSize) && (y + 1 < (int)Constants.RegionSize)) | ||
3739 | { | 2092 | { |
3740 | resultarr2[(y*2) + 1, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] + | 2093 | h0 = ((float)heights[iy]); // 0,0 vertice |
3741 | resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4); | 2094 | h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0 |
2095 | h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0 | ||
3742 | } | 2096 | } |
3743 | else | 2097 | else |
3744 | { | 2098 | { |
3745 | resultarr2[(y*2) + 1, (x*2) + 1] = resultarr[y, x]; | 2099 | h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice |
2100 | h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0 | ||
2101 | h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1 | ||
3746 | } | 2102 | } |
3747 | } | 2103 | */ |
3748 | } | 2104 | h0 = ((float)heights[iy]); // 0,0 vertice |
2105 | |||
2106 | if ((dy > dx)) | ||
2107 | { | ||
2108 | iy += regsize; | ||
2109 | h2 = (float)heights[iy]; // 0,1 vertice | ||
2110 | h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0 | ||
2111 | h2 = ((float)heights[iy + 1] - h2) * dx; // 1,1 vertice minus 0,1 | ||
3749 | } | 2112 | } |
3750 | //Flatten out the array | 2113 | else |
3751 | int i = 0; | ||
3752 | for (int y = 0; y < 512; y++) | ||
3753 | { | 2114 | { |
3754 | for (int x = 0; x < 512; x++) | 2115 | iy++; |
3755 | { | 2116 | h2 = (float)heights[iy]; // vertice 1,0 |
3756 | if (Single.IsNaN(resultarr2[y, x]) || Single.IsInfinity(resultarr2[y, x])) | 2117 | h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0 |
3757 | { | 2118 | h2 = (((float)heights[iy + regsize]) - h2) * dy; // 1,1 vertice minus 1,0 |
3758 | m_log.Warn("[ODE SCENE]: Non finite heightfield element detected. Setting it to 0"); | ||
3759 | resultarr2[y, x] = 0; | ||
3760 | } | ||
3761 | returnarr[i] = resultarr2[y, x]; | ||
3762 | i++; | ||
3763 | } | ||
3764 | } | 2119 | } |
3765 | 2120 | ||
3766 | return returnarr; | 2121 | return h0 + h1 + h2; |
3767 | } | 2122 | } |
3768 | 2123 | ||
3769 | #endregion | ||
3770 | 2124 | ||
3771 | public override void SetTerrain(float[] heightMap) | 2125 | public override void SetTerrain(float[] heightMap) |
3772 | { | 2126 | { |
@@ -3783,78 +2137,75 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
3783 | } | 2137 | } |
3784 | } | 2138 | } |
3785 | 2139 | ||
3786 | private void SetTerrain(float[] heightMap, Vector3 pOffset) | 2140 | public override void CombineTerrain(float[] heightMap, Vector3 pOffset) |
3787 | { | 2141 | { |
3788 | int startTime = Util.EnvironmentTickCount(); | 2142 | SetTerrain(heightMap, pOffset); |
3789 | m_log.DebugFormat("[ODE SCENE]: Setting terrain for {0} with offset {1}", Name, pOffset); | 2143 | } |
3790 | |||
3791 | // this._heightmap[i] = (double)heightMap[i]; | ||
3792 | // dbm (danx0r) -- creating a buffer zone of one extra sample all around | ||
3793 | //_origheightmap = heightMap; | ||
3794 | |||
3795 | float[] _heightmap; | ||
3796 | |||
3797 | // zero out a heightmap array float array (single dimension [flattened])) | ||
3798 | //if ((int)Constants.RegionSize == 256) | ||
3799 | // _heightmap = new float[514 * 514]; | ||
3800 | //else | ||
3801 | 2144 | ||
3802 | _heightmap = new float[(((int)Constants.RegionSize + 2) * ((int)Constants.RegionSize + 2))]; | 2145 | public void SetTerrain(float[] heightMap, Vector3 pOffset) |
2146 | { | ||
2147 | if (OdeUbitLib) | ||
2148 | UbitSetTerrain(heightMap, pOffset); | ||
2149 | else | ||
2150 | OriSetTerrain(heightMap, pOffset); | ||
2151 | } | ||
3803 | 2152 | ||
3804 | uint heightmapWidth = Constants.RegionSize + 1; | 2153 | public void OriSetTerrain(float[] heightMap, Vector3 pOffset) |
3805 | uint heightmapHeight = Constants.RegionSize + 1; | 2154 | { |
2155 | // assumes 1m size grid and constante size square regions | ||
2156 | // needs to know about sims around in future | ||
3806 | 2157 | ||
3807 | uint heightmapWidthSamples; | 2158 | float[] _heightmap; |
3808 | 2159 | ||
3809 | uint heightmapHeightSamples; | 2160 | uint heightmapWidth = Constants.RegionSize + 2; |
2161 | uint heightmapHeight = Constants.RegionSize + 2; | ||
3810 | 2162 | ||
3811 | //if (((int)Constants.RegionSize) == 256) | 2163 | uint heightmapWidthSamples = heightmapWidth + 1; |
3812 | //{ | 2164 | uint heightmapHeightSamples = heightmapHeight + 1; |
3813 | // heightmapWidthSamples = 2 * (uint)Constants.RegionSize + 2; | ||
3814 | // heightmapHeightSamples = 2 * (uint)Constants.RegionSize + 2; | ||
3815 | // heightmapWidth++; | ||
3816 | // heightmapHeight++; | ||
3817 | //} | ||
3818 | //else | ||
3819 | //{ | ||
3820 | 2165 | ||
3821 | heightmapWidthSamples = (uint)Constants.RegionSize + 1; | 2166 | _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples]; |
3822 | heightmapHeightSamples = (uint)Constants.RegionSize + 1; | ||
3823 | //} | ||
3824 | 2167 | ||
3825 | const float scale = 1.0f; | 2168 | const float scale = 1.0f; |
3826 | const float offset = 0.0f; | 2169 | const float offset = 0.0f; |
3827 | const float thickness = 0.2f; | 2170 | const float thickness = 10f; |
3828 | const int wrap = 0; | 2171 | const int wrap = 0; |
3829 | 2172 | ||
3830 | int regionsize = (int) Constants.RegionSize + 2; | 2173 | uint regionsize = Constants.RegionSize; |
3831 | //Double resolution | 2174 | |
3832 | //if (((int)Constants.RegionSize) == 256) | 2175 | float hfmin = float.MaxValue; |
3833 | // heightMap = ResizeTerrain512Interpolation(heightMap); | 2176 | float hfmax = float.MinValue; |
2177 | float val; | ||
2178 | uint xx; | ||
2179 | uint yy; | ||
3834 | 2180 | ||
2181 | uint maxXXYY = regionsize - 1; | ||
2182 | // flipping map adding one margin all around so things don't fall in edges | ||
3835 | 2183 | ||
3836 | // if (((int)Constants.RegionSize) == 256 && (int)Constants.RegionSize == 256) | 2184 | uint xt = 0; |
3837 | // regionsize = 512; | 2185 | xx = 0; |
3838 | 2186 | ||
3839 | float hfmin = 2000; | 2187 | for (uint x = 0; x < heightmapWidthSamples; x++) |
3840 | float hfmax = -2000; | ||
3841 | |||
3842 | for (int x = 0; x < heightmapWidthSamples; x++) | ||
3843 | { | 2188 | { |
3844 | for (int y = 0; y < heightmapHeightSamples; y++) | 2189 | if (x > 1 && xx < maxXXYY) |
2190 | xx++; | ||
2191 | yy = 0; | ||
2192 | for (uint y = 0; y < heightmapHeightSamples; y++) | ||
3845 | { | 2193 | { |
3846 | int xx = Util.Clip(x - 1, 0, regionsize - 1); | 2194 | if (y > 1 && y < maxXXYY) |
3847 | int yy = Util.Clip(y - 1, 0, regionsize - 1); | 2195 | yy += regionsize; |
3848 | 2196 | ||
3849 | 2197 | val = heightMap[yy + xx]; | |
3850 | float val= heightMap[yy * (int)Constants.RegionSize + xx]; | 2198 | if (val < 0.0f) |
3851 | _heightmap[x * ((int)Constants.RegionSize + 2) + y] = val; | 2199 | val = 0.0f; // no neg terrain as in chode |
3852 | 2200 | _heightmap[xt + y] = val; | |
3853 | hfmin = (val < hfmin) ? val : hfmin; | 2201 | |
3854 | hfmax = (val > hfmax) ? val : hfmax; | 2202 | if (hfmin > val) |
2203 | hfmin = val; | ||
2204 | if (hfmax < val) | ||
2205 | hfmax = val; | ||
3855 | } | 2206 | } |
2207 | xt += heightmapHeightSamples; | ||
3856 | } | 2208 | } |
3857 | |||
3858 | lock (OdeLock) | 2209 | lock (OdeLock) |
3859 | { | 2210 | { |
3860 | IntPtr GroundGeom = IntPtr.Zero; | 2211 | IntPtr GroundGeom = IntPtr.Zero; |
@@ -3863,62 +2214,177 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
3863 | RegionTerrain.Remove(pOffset); | 2214 | RegionTerrain.Remove(pOffset); |
3864 | if (GroundGeom != IntPtr.Zero) | 2215 | if (GroundGeom != IntPtr.Zero) |
3865 | { | 2216 | { |
2217 | actor_name_map.Remove(GroundGeom); | ||
2218 | d.GeomDestroy(GroundGeom); | ||
2219 | |||
3866 | if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) | 2220 | if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) |
3867 | { | 2221 | { |
2222 | TerrainHeightFieldHeightsHandlers[GroundGeom].Free(); | ||
2223 | TerrainHeightFieldHeightsHandlers.Remove(GroundGeom); | ||
3868 | TerrainHeightFieldHeights.Remove(GroundGeom); | 2224 | TerrainHeightFieldHeights.Remove(GroundGeom); |
3869 | } | 2225 | } |
3870 | d.SpaceRemove(space, GroundGeom); | ||
3871 | d.GeomDestroy(GroundGeom); | ||
3872 | } | 2226 | } |
3873 | |||
3874 | } | 2227 | } |
3875 | IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); | 2228 | IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); |
3876 | d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0, heightmapWidth + 1, heightmapHeight + 1, | 2229 | |
3877 | (int)heightmapWidthSamples + 1, (int)heightmapHeightSamples + 1, scale, | 2230 | GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned); |
2231 | |||
2232 | d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, heightmapWidth , heightmapHeight, | ||
2233 | (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale, | ||
3878 | offset, thickness, wrap); | 2234 | offset, thickness, wrap); |
2235 | |||
3879 | d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); | 2236 | d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); |
3880 | GroundGeom = d.CreateHeightfield(space, HeightmapData, 1); | 2237 | |
2238 | GroundGeom = d.CreateHeightfield(GroundSpace, HeightmapData, 1); | ||
2239 | |||
3881 | if (GroundGeom != IntPtr.Zero) | 2240 | if (GroundGeom != IntPtr.Zero) |
3882 | { | 2241 | { |
3883 | d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land)); | 2242 | d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land)); |
3884 | d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space)); | 2243 | d.GeomSetCollideBits(GroundGeom, 0); |
2244 | |||
2245 | PhysicsActor pa = new NullPhysicsActor(); | ||
2246 | pa.Name = "Terrain"; | ||
2247 | pa.PhysicsActorType = (int)ActorTypes.Ground; | ||
2248 | actor_name_map[GroundGeom] = pa; | ||
2249 | |||
2250 | // geom_name_map[GroundGeom] = "Terrain"; | ||
2251 | |||
2252 | d.Matrix3 R = new d.Matrix3(); | ||
2253 | |||
2254 | Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); | ||
2255 | Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); | ||
3885 | 2256 | ||
2257 | |||
2258 | q1 = q1 * q2; | ||
2259 | |||
2260 | Vector3 v3; | ||
2261 | float angle; | ||
2262 | q1.GetAxisAngle(out v3, out angle); | ||
2263 | |||
2264 | d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); | ||
2265 | d.GeomSetRotation(GroundGeom, ref R); | ||
2266 | d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0); | ||
2267 | RegionTerrain.Add(pOffset, GroundGeom); | ||
2268 | TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); | ||
2269 | TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); | ||
3886 | } | 2270 | } |
3887 | geom_name_map[GroundGeom] = "Terrain"; | 2271 | } |
2272 | } | ||
3888 | 2273 | ||
3889 | d.Matrix3 R = new d.Matrix3(); | 2274 | public void UbitSetTerrain(float[] heightMap, Vector3 pOffset) |
2275 | { | ||
2276 | // assumes 1m size grid and constante size square regions | ||
2277 | // needs to know about sims around in future | ||
3890 | 2278 | ||
3891 | Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); | 2279 | float[] _heightmap; |
3892 | Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); | 2280 | |
3893 | //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1)); | 2281 | uint heightmapWidth = Constants.RegionSize + 2; |
2282 | uint heightmapHeight = Constants.RegionSize + 2; | ||
2283 | |||
2284 | uint heightmapWidthSamples = heightmapWidth + 1; | ||
2285 | uint heightmapHeightSamples = heightmapHeight + 1; | ||
2286 | |||
2287 | _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples]; | ||
2288 | |||
2289 | |||
2290 | uint regionsize = Constants.RegionSize; | ||
3894 | 2291 | ||
3895 | q1 = q1 * q2; | 2292 | float hfmin = float.MaxValue; |
3896 | //q1 = q1 * q3; | 2293 | // float hfmax = float.MinValue; |
3897 | Vector3 v3; | 2294 | float val; |
3898 | float angle; | ||
3899 | q1.GetAxisAngle(out v3, out angle); | ||
3900 | 2295 | ||
3901 | d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); | 2296 | |
3902 | d.GeomSetRotation(GroundGeom, ref R); | 2297 | uint maxXXYY = regionsize - 1; |
3903 | d.GeomSetPosition(GroundGeom, (pOffset.X + ((int)Constants.RegionSize * 0.5f)), (pOffset.Y + ((int)Constants.RegionSize * 0.5f)), 0); | 2298 | // adding one margin all around so things don't fall in edges |
3904 | IntPtr testGround = IntPtr.Zero; | 2299 | |
3905 | if (RegionTerrain.TryGetValue(pOffset, out testGround)) | 2300 | uint xx; |
2301 | uint yy = 0; | ||
2302 | uint yt = 0; | ||
2303 | |||
2304 | for (uint y = 0; y < heightmapHeightSamples; y++) | ||
2305 | { | ||
2306 | if (y > 1 && y < maxXXYY) | ||
2307 | yy += regionsize; | ||
2308 | xx = 0; | ||
2309 | for (uint x = 0; x < heightmapWidthSamples; x++) | ||
3906 | { | 2310 | { |
3907 | RegionTerrain.Remove(pOffset); | 2311 | if (x > 1 && x < maxXXYY) |
2312 | xx++; | ||
2313 | |||
2314 | val = heightMap[yy + xx]; | ||
2315 | if (val < 0.0f) | ||
2316 | val = 0.0f; // no neg terrain as in chode | ||
2317 | _heightmap[yt + x] = val; | ||
2318 | |||
2319 | if (hfmin > val) | ||
2320 | hfmin = val; | ||
2321 | // if (hfmax < val) | ||
2322 | // hfmax = val; | ||
3908 | } | 2323 | } |
3909 | RegionTerrain.Add(pOffset, GroundGeom, GroundGeom); | 2324 | yt += heightmapWidthSamples; |
3910 | TerrainHeightFieldHeights.Add(GroundGeom,_heightmap); | ||
3911 | } | 2325 | } |
2326 | lock (OdeLock) | ||
2327 | { | ||
2328 | IntPtr GroundGeom = IntPtr.Zero; | ||
2329 | if (RegionTerrain.TryGetValue(pOffset, out GroundGeom)) | ||
2330 | { | ||
2331 | RegionTerrain.Remove(pOffset); | ||
2332 | if (GroundGeom != IntPtr.Zero) | ||
2333 | { | ||
2334 | actor_name_map.Remove(GroundGeom); | ||
2335 | d.GeomDestroy(GroundGeom); | ||
2336 | |||
2337 | if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) | ||
2338 | { | ||
2339 | if (TerrainHeightFieldHeightsHandlers[GroundGeom].IsAllocated) | ||
2340 | TerrainHeightFieldHeightsHandlers[GroundGeom].Free(); | ||
2341 | TerrainHeightFieldHeightsHandlers.Remove(GroundGeom); | ||
2342 | TerrainHeightFieldHeights.Remove(GroundGeom); | ||
2343 | } | ||
2344 | } | ||
2345 | } | ||
2346 | IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); | ||
2347 | |||
2348 | const int wrap = 0; | ||
2349 | float thickness = hfmin; | ||
2350 | if (thickness < 0) | ||
2351 | thickness = 1; | ||
2352 | |||
2353 | GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned); | ||
3912 | 2354 | ||
3913 | m_log.DebugFormat( | 2355 | d.GeomUbitTerrainDataBuild(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, 1.0f, |
3914 | "[ODE SCENE]: Setting terrain for {0} took {1}ms", Name, Util.EnvironmentTickCountSubtract(startTime)); | 2356 | (int)heightmapWidthSamples, (int)heightmapHeightSamples, |
2357 | thickness, wrap); | ||
2358 | |||
2359 | // d.GeomUbitTerrainDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); | ||
2360 | GroundGeom = d.CreateUbitTerrain(GroundSpace, HeightmapData, 1); | ||
2361 | if (GroundGeom != IntPtr.Zero) | ||
2362 | { | ||
2363 | d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land)); | ||
2364 | d.GeomSetCollideBits(GroundGeom, 0); | ||
2365 | |||
2366 | |||
2367 | PhysicsActor pa = new NullPhysicsActor(); | ||
2368 | pa.Name = "Terrain"; | ||
2369 | pa.PhysicsActorType = (int)ActorTypes.Ground; | ||
2370 | actor_name_map[GroundGeom] = pa; | ||
2371 | |||
2372 | // geom_name_map[GroundGeom] = "Terrain"; | ||
2373 | |||
2374 | d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0); | ||
2375 | RegionTerrain.Add(pOffset, GroundGeom); | ||
2376 | TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); | ||
2377 | TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); | ||
2378 | } | ||
2379 | } | ||
3915 | } | 2380 | } |
3916 | 2381 | ||
2382 | |||
3917 | public override void DeleteTerrain() | 2383 | public override void DeleteTerrain() |
3918 | { | 2384 | { |
3919 | } | 2385 | } |
3920 | 2386 | ||
3921 | internal float GetWaterLevel() | 2387 | public float GetWaterLevel() |
3922 | { | 2388 | { |
3923 | return waterlevel; | 2389 | return waterlevel; |
3924 | } | 2390 | } |
@@ -3927,169 +2393,252 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
3927 | { | 2393 | { |
3928 | return true; | 2394 | return true; |
3929 | } | 2395 | } |
2396 | /* | ||
2397 | public override void UnCombine(PhysicsScene pScene) | ||
2398 | { | ||
2399 | IntPtr localGround = IntPtr.Zero; | ||
2400 | // float[] localHeightfield; | ||
2401 | bool proceed = false; | ||
2402 | List<IntPtr> geomDestroyList = new List<IntPtr>(); | ||
3930 | 2403 | ||
3931 | // public override void UnCombine(PhysicsScene pScene) | 2404 | lock (OdeLock) |
3932 | // { | 2405 | { |
3933 | // IntPtr localGround = IntPtr.Zero; | 2406 | if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround)) |
3934 | //// float[] localHeightfield; | 2407 | { |
3935 | // bool proceed = false; | 2408 | foreach (IntPtr geom in TerrainHeightFieldHeights.Keys) |
3936 | // List<IntPtr> geomDestroyList = new List<IntPtr>(); | 2409 | { |
3937 | // | 2410 | if (geom == localGround) |
3938 | // lock (OdeLock) | 2411 | { |
3939 | // { | 2412 | // localHeightfield = TerrainHeightFieldHeights[geom]; |
3940 | // if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround)) | 2413 | proceed = true; |
3941 | // { | 2414 | } |
3942 | // foreach (IntPtr geom in TerrainHeightFieldHeights.Keys) | 2415 | else |
3943 | // { | 2416 | { |
3944 | // if (geom == localGround) | 2417 | geomDestroyList.Add(geom); |
3945 | // { | 2418 | } |
3946 | //// localHeightfield = TerrainHeightFieldHeights[geom]; | 2419 | } |
3947 | // proceed = true; | 2420 | |
3948 | // } | 2421 | if (proceed) |
3949 | // else | 2422 | { |
3950 | // { | 2423 | m_worldOffset = Vector3.Zero; |
3951 | // geomDestroyList.Add(geom); | 2424 | WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); |
3952 | // } | 2425 | m_parentScene = null; |
3953 | // } | 2426 | |
3954 | // | 2427 | foreach (IntPtr g in geomDestroyList) |
3955 | // if (proceed) | 2428 | { |
3956 | // { | 2429 | // removingHeightField needs to be done or the garbage collector will |
3957 | // m_worldOffset = Vector3.Zero; | 2430 | // collect the terrain data before we tell ODE to destroy it causing |
3958 | // WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); | 2431 | // memory corruption |
3959 | // m_parentScene = null; | 2432 | if (TerrainHeightFieldHeights.ContainsKey(g)) |
3960 | // | 2433 | { |
3961 | // foreach (IntPtr g in geomDestroyList) | 2434 | // float[] removingHeightField = TerrainHeightFieldHeights[g]; |
3962 | // { | 2435 | TerrainHeightFieldHeights.Remove(g); |
3963 | // // removingHeightField needs to be done or the garbage collector will | 2436 | |
3964 | // // collect the terrain data before we tell ODE to destroy it causing | 2437 | if (RegionTerrain.ContainsKey(g)) |
3965 | // // memory corruption | 2438 | { |
3966 | // if (TerrainHeightFieldHeights.ContainsKey(g)) | 2439 | RegionTerrain.Remove(g); |
3967 | // { | 2440 | } |
3968 | //// float[] removingHeightField = TerrainHeightFieldHeights[g]; | 2441 | |
3969 | // TerrainHeightFieldHeights.Remove(g); | 2442 | d.GeomDestroy(g); |
3970 | // | 2443 | //removingHeightField = new float[0]; |
3971 | // if (RegionTerrain.ContainsKey(g)) | 2444 | } |
3972 | // { | 2445 | } |
3973 | // RegionTerrain.Remove(g); | ||
3974 | // } | ||
3975 | // | ||
3976 | // d.GeomDestroy(g); | ||
3977 | // //removingHeightField = new float[0]; | ||
3978 | // } | ||
3979 | // } | ||
3980 | // | ||
3981 | // } | ||
3982 | // else | ||
3983 | // { | ||
3984 | // m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data."); | ||
3985 | // } | ||
3986 | // } | ||
3987 | // } | ||
3988 | // } | ||
3989 | 2446 | ||
2447 | } | ||
2448 | else | ||
2449 | { | ||
2450 | m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data."); | ||
2451 | } | ||
2452 | } | ||
2453 | } | ||
2454 | } | ||
2455 | */ | ||
3990 | public override void SetWaterLevel(float baseheight) | 2456 | public override void SetWaterLevel(float baseheight) |
3991 | { | 2457 | { |
3992 | waterlevel = baseheight; | 2458 | waterlevel = baseheight; |
3993 | randomizeWater(waterlevel); | 2459 | // randomizeWater(waterlevel); |
3994 | } | 2460 | } |
3995 | 2461 | /* | |
3996 | private void randomizeWater(float baseheight) | 2462 | public void randomizeWater(float baseheight) |
3997 | { | 2463 | { |
3998 | const uint heightmapWidth = m_regionWidth + 2; | 2464 | const uint heightmapWidth = Constants.RegionSize + 2; |
3999 | const uint heightmapHeight = m_regionHeight + 2; | 2465 | const uint heightmapHeight = Constants.RegionSize + 2; |
4000 | const uint heightmapWidthSamples = m_regionWidth + 2; | 2466 | const uint heightmapWidthSamples = heightmapWidth + 1; |
4001 | const uint heightmapHeightSamples = m_regionHeight + 2; | 2467 | const uint heightmapHeightSamples = heightmapHeight + 1; |
2468 | |||
4002 | const float scale = 1.0f; | 2469 | const float scale = 1.0f; |
4003 | const float offset = 0.0f; | 2470 | const float offset = 0.0f; |
4004 | const float thickness = 2.9f; | ||
4005 | const int wrap = 0; | 2471 | const int wrap = 0; |
4006 | 2472 | ||
4007 | for (int i = 0; i < (258 * 258); i++) | 2473 | float[] _watermap = new float[heightmapWidthSamples * heightmapWidthSamples]; |
2474 | |||
2475 | float maxheigh = float.MinValue; | ||
2476 | float minheigh = float.MaxValue; | ||
2477 | float val; | ||
2478 | for (int i = 0; i < (heightmapWidthSamples * heightmapHeightSamples); i++) | ||
4008 | { | 2479 | { |
4009 | _watermap[i] = (baseheight-0.1f) + ((float)fluidRandomizer.Next(1,9) / 10f); | 2480 | |
4010 | // m_log.Info((baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f)); | 2481 | val = (baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f); |
2482 | _watermap[i] = val; | ||
2483 | if (maxheigh < val) | ||
2484 | maxheigh = val; | ||
2485 | if (minheigh > val) | ||
2486 | minheigh = val; | ||
4011 | } | 2487 | } |
4012 | 2488 | ||
2489 | float thickness = minheigh; | ||
2490 | |||
4013 | lock (OdeLock) | 2491 | lock (OdeLock) |
4014 | { | 2492 | { |
4015 | if (WaterGeom != IntPtr.Zero) | 2493 | if (WaterGeom != IntPtr.Zero) |
4016 | { | 2494 | { |
4017 | d.SpaceRemove(space, WaterGeom); | 2495 | actor_name_map.Remove(WaterGeom); |
2496 | d.GeomDestroy(WaterGeom); | ||
2497 | d.GeomHeightfieldDataDestroy(WaterHeightmapData); | ||
2498 | WaterGeom = IntPtr.Zero; | ||
2499 | WaterHeightmapData = IntPtr.Zero; | ||
2500 | if(WaterMapHandler.IsAllocated) | ||
2501 | WaterMapHandler.Free(); | ||
4018 | } | 2502 | } |
4019 | IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); | 2503 | |
4020 | d.GeomHeightfieldDataBuildSingle(HeightmapData, _watermap, 0, heightmapWidth, heightmapHeight, | 2504 | WaterHeightmapData = d.GeomHeightfieldDataCreate(); |
2505 | |||
2506 | WaterMapHandler = GCHandle.Alloc(_watermap, GCHandleType.Pinned); | ||
2507 | |||
2508 | d.GeomHeightfieldDataBuildSingle(WaterHeightmapData, WaterMapHandler.AddrOfPinnedObject(), 0, heightmapWidth, heightmapHeight, | ||
4021 | (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale, | 2509 | (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale, |
4022 | offset, thickness, wrap); | 2510 | offset, thickness, wrap); |
4023 | d.GeomHeightfieldDataSetBounds(HeightmapData, m_regionWidth, m_regionHeight); | 2511 | d.GeomHeightfieldDataSetBounds(WaterHeightmapData, minheigh, maxheigh); |
4024 | WaterGeom = d.CreateHeightfield(space, HeightmapData, 1); | 2512 | WaterGeom = d.CreateHeightfield(StaticSpace, WaterHeightmapData, 1); |
4025 | if (WaterGeom != IntPtr.Zero) | 2513 | if (WaterGeom != IntPtr.Zero) |
4026 | { | 2514 | { |
4027 | d.GeomSetCategoryBits(WaterGeom, (int)(CollisionCategories.Water)); | 2515 | d.GeomSetCategoryBits(WaterGeom, (uint)(CollisionCategories.Water)); |
4028 | d.GeomSetCollideBits(WaterGeom, (int)(CollisionCategories.Space)); | 2516 | d.GeomSetCollideBits(WaterGeom, 0); |
4029 | } | 2517 | |
4030 | 2518 | ||
4031 | geom_name_map[WaterGeom] = "Water"; | 2519 | PhysicsActor pa = new NullPhysicsActor(); |
2520 | pa.Name = "Water"; | ||
2521 | pa.PhysicsActorType = (int)ActorTypes.Water; | ||
4032 | 2522 | ||
4033 | d.Matrix3 R = new d.Matrix3(); | 2523 | actor_name_map[WaterGeom] = pa; |
2524 | // geom_name_map[WaterGeom] = "Water"; | ||
4034 | 2525 | ||
4035 | Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); | 2526 | d.Matrix3 R = new d.Matrix3(); |
4036 | Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); | ||
4037 | //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1)); | ||
4038 | 2527 | ||
4039 | q1 = q1 * q2; | 2528 | Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); |
4040 | //q1 = q1 * q3; | 2529 | Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); |
4041 | Vector3 v3; | ||
4042 | float angle; | ||
4043 | q1.GetAxisAngle(out v3, out angle); | ||
4044 | 2530 | ||
4045 | d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); | 2531 | q1 = q1 * q2; |
4046 | d.GeomSetRotation(WaterGeom, ref R); | 2532 | Vector3 v3; |
4047 | d.GeomSetPosition(WaterGeom, 128, 128, 0); | 2533 | float angle; |
2534 | q1.GetAxisAngle(out v3, out angle); | ||
2535 | |||
2536 | d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); | ||
2537 | d.GeomSetRotation(WaterGeom, ref R); | ||
2538 | d.GeomSetPosition(WaterGeom, (float)Constants.RegionSize * 0.5f, (float)Constants.RegionSize * 0.5f, 0); | ||
2539 | } | ||
4048 | } | 2540 | } |
4049 | } | 2541 | } |
4050 | 2542 | */ | |
4051 | public override void Dispose() | 2543 | public override void Dispose() |
4052 | { | 2544 | { |
4053 | _worldInitialized = false; | 2545 | if (m_meshWorker != null) |
4054 | 2546 | m_meshWorker.Stop(); | |
4055 | m_rayCastManager.Dispose(); | ||
4056 | m_rayCastManager = null; | ||
4057 | 2547 | ||
4058 | lock (OdeLock) | 2548 | lock (OdeLock) |
4059 | { | 2549 | { |
2550 | m_rayCastManager.Dispose(); | ||
2551 | m_rayCastManager = null; | ||
2552 | |||
4060 | lock (_prims) | 2553 | lock (_prims) |
4061 | { | 2554 | { |
2555 | ChangesQueue.Clear(); | ||
4062 | foreach (OdePrim prm in _prims) | 2556 | foreach (OdePrim prm in _prims) |
4063 | { | 2557 | { |
4064 | RemovePrim(prm); | 2558 | prm.DoAChange(changes.Remove, null); |
2559 | _collisionEventPrim.Remove(prm); | ||
4065 | } | 2560 | } |
2561 | _prims.Clear(); | ||
2562 | } | ||
2563 | |||
2564 | OdeCharacter[] chtorem; | ||
2565 | lock (_characters) | ||
2566 | { | ||
2567 | chtorem = new OdeCharacter[_characters.Count]; | ||
2568 | _characters.CopyTo(chtorem); | ||
4066 | } | 2569 | } |
4067 | 2570 | ||
4068 | //foreach (OdeCharacter act in _characters) | 2571 | ChangesQueue.Clear(); |
4069 | //{ | 2572 | foreach (OdeCharacter ch in chtorem) |
4070 | //RemoveAvatar(act); | 2573 | ch.DoAChange(changes.Remove, null); |
4071 | //} | 2574 | |
2575 | |||
2576 | foreach (IntPtr GroundGeom in RegionTerrain.Values) | ||
2577 | { | ||
2578 | if (GroundGeom != IntPtr.Zero) | ||
2579 | d.GeomDestroy(GroundGeom); | ||
2580 | } | ||
2581 | |||
2582 | |||
2583 | RegionTerrain.Clear(); | ||
2584 | |||
2585 | if (TerrainHeightFieldHeightsHandlers.Count > 0) | ||
2586 | { | ||
2587 | foreach (GCHandle gch in TerrainHeightFieldHeightsHandlers.Values) | ||
2588 | { | ||
2589 | if (gch.IsAllocated) | ||
2590 | gch.Free(); | ||
2591 | } | ||
2592 | } | ||
2593 | |||
2594 | TerrainHeightFieldHeightsHandlers.Clear(); | ||
2595 | TerrainHeightFieldHeights.Clear(); | ||
2596 | /* | ||
2597 | if (WaterGeom != IntPtr.Zero) | ||
2598 | { | ||
2599 | d.GeomDestroy(WaterGeom); | ||
2600 | WaterGeom = IntPtr.Zero; | ||
2601 | if (WaterHeightmapData != IntPtr.Zero) | ||
2602 | d.GeomHeightfieldDataDestroy(WaterHeightmapData); | ||
2603 | WaterHeightmapData = IntPtr.Zero; | ||
2604 | |||
2605 | if (WaterMapHandler.IsAllocated) | ||
2606 | WaterMapHandler.Free(); | ||
2607 | } | ||
2608 | */ | ||
2609 | if (ContactgeomsArray != IntPtr.Zero) | ||
2610 | Marshal.FreeHGlobal(ContactgeomsArray); | ||
2611 | if (GlobalContactsArray != IntPtr.Zero) | ||
2612 | Marshal.FreeHGlobal(GlobalContactsArray); | ||
2613 | |||
2614 | |||
4072 | d.WorldDestroy(world); | 2615 | d.WorldDestroy(world); |
2616 | world = IntPtr.Zero; | ||
4073 | //d.CloseODE(); | 2617 | //d.CloseODE(); |
4074 | } | 2618 | } |
4075 | |||
4076 | } | 2619 | } |
4077 | 2620 | ||
4078 | public override Dictionary<uint, float> GetTopColliders() | 2621 | public override Dictionary<uint, float> GetTopColliders() |
4079 | { | 2622 | { |
4080 | Dictionary<uint, float> topColliders; | 2623 | Dictionary<uint, float> returncolliders = new Dictionary<uint, float>(); |
4081 | 2624 | int cnt = 0; | |
4082 | lock (_prims) | 2625 | lock (_prims) |
4083 | { | 2626 | { |
4084 | List<OdePrim> orderedPrims = new List<OdePrim>(_prims); | 2627 | foreach (OdePrim prm in _prims) |
4085 | orderedPrims.OrderByDescending(p => p.CollisionScore).Take(25); | 2628 | { |
4086 | topColliders = orderedPrims.ToDictionary(p => p.LocalID, p => p.CollisionScore); | 2629 | if (prm.CollisionScore > 0) |
4087 | 2630 | { | |
4088 | foreach (OdePrim p in _prims) | 2631 | returncolliders.Add(prm.LocalID, prm.CollisionScore); |
4089 | p.CollisionScore = 0; | 2632 | cnt++; |
2633 | prm.CollisionScore = 0f; | ||
2634 | if (cnt > 25) | ||
2635 | { | ||
2636 | break; | ||
2637 | } | ||
2638 | } | ||
2639 | } | ||
4090 | } | 2640 | } |
4091 | 2641 | return returncolliders; | |
4092 | return topColliders; | ||
4093 | } | 2642 | } |
4094 | 2643 | ||
4095 | public override bool SupportsRayCast() | 2644 | public override bool SupportsRayCast() |
@@ -4113,6 +2662,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
4113 | } | 2662 | } |
4114 | } | 2663 | } |
4115 | 2664 | ||
2665 | // don't like this | ||
4116 | public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count) | 2666 | public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count) |
4117 | { | 2667 | { |
4118 | ContactResult[] ourResults = null; | 2668 | ContactResult[] ourResults = null; |
@@ -4129,182 +2679,107 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
4129 | waitTime++; | 2679 | waitTime++; |
4130 | } | 2680 | } |
4131 | if (ourResults == null) | 2681 | if (ourResults == null) |
4132 | return new List<ContactResult> (); | 2682 | return new List<ContactResult>(); |
4133 | return new List<ContactResult>(ourResults); | 2683 | return new List<ContactResult>(ourResults); |
4134 | } | 2684 | } |
4135 | 2685 | ||
4136 | #if USE_DRAWSTUFF | 2686 | public override bool SuportsRaycastWorldFiltered() |
4137 | // Keyboard callback | ||
4138 | public void command(int cmd) | ||
4139 | { | 2687 | { |
4140 | IntPtr geom; | 2688 | return true; |
4141 | d.Mass mass; | ||
4142 | d.Vector3 sides = new d.Vector3(d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f); | ||
4143 | |||
4144 | |||
4145 | |||
4146 | Char ch = Char.ToLower((Char)cmd); | ||
4147 | switch ((Char)ch) | ||
4148 | { | ||
4149 | case 'w': | ||
4150 | try | ||
4151 | { | ||
4152 | Vector3 rotate = (new Vector3(1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD)); | ||
4153 | |||
4154 | xyz.X += rotate.X; xyz.Y += rotate.Y; xyz.Z += rotate.Z; | ||
4155 | ds.SetViewpoint(ref xyz, ref hpr); | ||
4156 | } | ||
4157 | catch (ArgumentException) | ||
4158 | { hpr.X = 0; } | ||
4159 | break; | ||
4160 | |||
4161 | case 'a': | ||
4162 | hpr.X++; | ||
4163 | ds.SetViewpoint(ref xyz, ref hpr); | ||
4164 | break; | ||
4165 | |||
4166 | case 's': | ||
4167 | try | ||
4168 | { | ||
4169 | Vector3 rotate2 = (new Vector3(-1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD)); | ||
4170 | |||
4171 | xyz.X += rotate2.X; xyz.Y += rotate2.Y; xyz.Z += rotate2.Z; | ||
4172 | ds.SetViewpoint(ref xyz, ref hpr); | ||
4173 | } | ||
4174 | catch (ArgumentException) | ||
4175 | { hpr.X = 0; } | ||
4176 | break; | ||
4177 | case 'd': | ||
4178 | hpr.X--; | ||
4179 | ds.SetViewpoint(ref xyz, ref hpr); | ||
4180 | break; | ||
4181 | case 'r': | ||
4182 | xyz.Z++; | ||
4183 | ds.SetViewpoint(ref xyz, ref hpr); | ||
4184 | break; | ||
4185 | case 'f': | ||
4186 | xyz.Z--; | ||
4187 | ds.SetViewpoint(ref xyz, ref hpr); | ||
4188 | break; | ||
4189 | case 'e': | ||
4190 | xyz.Y++; | ||
4191 | ds.SetViewpoint(ref xyz, ref hpr); | ||
4192 | break; | ||
4193 | case 'q': | ||
4194 | xyz.Y--; | ||
4195 | ds.SetViewpoint(ref xyz, ref hpr); | ||
4196 | break; | ||
4197 | } | ||
4198 | } | 2689 | } |
4199 | 2690 | ||
4200 | public void step(int pause) | 2691 | public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter) |
4201 | { | 2692 | { |
4202 | 2693 | object SyncObject = new object(); | |
4203 | ds.SetColor(1.0f, 1.0f, 0.0f); | 2694 | List<ContactResult> ourresults = new List<ContactResult>(); |
4204 | ds.SetTexture(ds.Texture.Wood); | 2695 | |
4205 | lock (_prims) | 2696 | RayCallback retMethod = delegate(List<ContactResult> results) |
4206 | { | 2697 | { |
4207 | foreach (OdePrim prm in _prims) | 2698 | lock (SyncObject) |
4208 | { | 2699 | { |
4209 | //IntPtr body = d.GeomGetBody(prm.prim_geom); | 2700 | ourresults = results; |
4210 | if (prm.prim_geom != IntPtr.Zero) | 2701 | Monitor.PulseAll(SyncObject); |
4211 | { | ||
4212 | d.Vector3 pos; | ||
4213 | d.GeomCopyPosition(prm.prim_geom, out pos); | ||
4214 | //d.BodyCopyPosition(body, out pos); | ||
4215 | |||
4216 | d.Matrix3 R; | ||
4217 | d.GeomCopyRotation(prm.prim_geom, out R); | ||
4218 | //d.BodyCopyRotation(body, out R); | ||
4219 | |||
4220 | |||
4221 | d.Vector3 sides = new d.Vector3(); | ||
4222 | sides.X = prm.Size.X; | ||
4223 | sides.Y = prm.Size.Y; | ||
4224 | sides.Z = prm.Size.Z; | ||
4225 | |||
4226 | ds.DrawBox(ref pos, ref R, ref sides); | ||
4227 | } | ||
4228 | } | 2702 | } |
4229 | } | 2703 | }; |
4230 | ds.SetColor(1.0f, 0.0f, 0.0f); | ||
4231 | 2704 | ||
4232 | foreach (OdeCharacter chr in _characters) | 2705 | lock (SyncObject) |
4233 | { | 2706 | { |
4234 | if (chr.Shell != IntPtr.Zero) | 2707 | m_rayCastManager.QueueRequest(position, direction, length, Count,filter, retMethod); |
4235 | { | 2708 | if (!Monitor.Wait(SyncObject, 500)) |
4236 | IntPtr body = d.GeomGetBody(chr.Shell); | 2709 | return null; |
4237 | 2710 | else | |
4238 | d.Vector3 pos; | 2711 | return ourresults; |
4239 | d.GeomCopyPosition(chr.Shell, out pos); | ||
4240 | //d.BodyCopyPosition(body, out pos); | ||
4241 | |||
4242 | d.Matrix3 R; | ||
4243 | d.GeomCopyRotation(chr.Shell, out R); | ||
4244 | //d.BodyCopyRotation(body, out R); | ||
4245 | |||
4246 | ds.DrawCapsule(ref pos, ref R, chr.Size.Z, 0.35f); | ||
4247 | d.Vector3 sides = new d.Vector3(); | ||
4248 | sides.X = 0.5f; | ||
4249 | sides.Y = 0.5f; | ||
4250 | sides.Z = 0.5f; | ||
4251 | |||
4252 | ds.DrawBox(ref pos, ref R, ref sides); | ||
4253 | } | ||
4254 | } | 2712 | } |
4255 | } | 2713 | } |
4256 | 2714 | ||
4257 | public void start(int unused) | 2715 | public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) |
4258 | { | 2716 | { |
4259 | ds.SetViewpoint(ref xyz, ref hpr); | 2717 | if (retMethod != null && actor !=null) |
2718 | { | ||
2719 | IntPtr geom; | ||
2720 | if (actor is OdePrim) | ||
2721 | geom = ((OdePrim)actor).prim_geom; | ||
2722 | else if (actor is OdeCharacter) | ||
2723 | geom = ((OdePrim)actor).prim_geom; | ||
2724 | else | ||
2725 | return; | ||
2726 | if (geom == IntPtr.Zero) | ||
2727 | return; | ||
2728 | m_rayCastManager.QueueRequest(geom, position, direction, length, retMethod); | ||
2729 | } | ||
4260 | } | 2730 | } |
4261 | #endif | ||
4262 | 2731 | ||
4263 | public override Dictionary<string, float> GetStats() | 2732 | public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) |
4264 | { | 2733 | { |
4265 | if (!CollectStats) | 2734 | if (retMethod != null && actor != null) |
4266 | return null; | ||
4267 | |||
4268 | Dictionary<string, float> returnStats; | ||
4269 | |||
4270 | lock (OdeLock) | ||
4271 | { | 2735 | { |
4272 | returnStats = new Dictionary<string, float>(m_stats); | 2736 | IntPtr geom; |
4273 | 2737 | if (actor is OdePrim) | |
4274 | // FIXME: This is a SUPER DUMB HACK until we can establish stats that aren't subject to a division by | 2738 | geom = ((OdePrim)actor).prim_geom; |
4275 | // 3 from the SimStatsReporter. | 2739 | else if (actor is OdeCharacter) |
4276 | returnStats[ODETotalAvatarsStatName] = _characters.Count * 3; | 2740 | geom = ((OdePrim)actor).prim_geom; |
4277 | returnStats[ODETotalPrimsStatName] = _prims.Count * 3; | 2741 | else |
4278 | returnStats[ODEActivePrimsStatName] = _activeprims.Count * 3; | 2742 | return; |
2743 | if (geom == IntPtr.Zero) | ||
2744 | return; | ||
4279 | 2745 | ||
4280 | InitializeExtraStats(); | 2746 | m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod); |
4281 | } | 2747 | } |
4282 | |||
4283 | returnStats[ODEOtherCollisionFrameMsStatName] | ||
4284 | = returnStats[ODEOtherCollisionFrameMsStatName] | ||
4285 | - returnStats[ODENativeSpaceCollisionFrameMsStatName] | ||
4286 | - returnStats[ODENativeGeomCollisionFrameMsStatName]; | ||
4287 | |||
4288 | return returnStats; | ||
4289 | } | 2748 | } |
4290 | 2749 | ||
4291 | private void InitializeExtraStats() | 2750 | // don't like this |
2751 | public override List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count) | ||
4292 | { | 2752 | { |
4293 | m_stats[ODETotalFrameMsStatName] = 0; | 2753 | if (actor != null) |
4294 | m_stats[ODEAvatarTaintMsStatName] = 0; | 2754 | { |
4295 | m_stats[ODEPrimTaintMsStatName] = 0; | 2755 | IntPtr geom; |
4296 | m_stats[ODEAvatarForcesFrameMsStatName] = 0; | 2756 | if (actor is OdePrim) |
4297 | m_stats[ODEPrimForcesFrameMsStatName] = 0; | 2757 | geom = ((OdePrim)actor).prim_geom; |
4298 | m_stats[ODERaycastingFrameMsStatName] = 0; | 2758 | else if (actor is OdeCharacter) |
4299 | m_stats[ODENativeStepFrameMsStatName] = 0; | 2759 | geom = ((OdePrim)actor).prim_geom; |
4300 | m_stats[ODENativeSpaceCollisionFrameMsStatName] = 0; | 2760 | else |
4301 | m_stats[ODENativeGeomCollisionFrameMsStatName] = 0; | 2761 | return new List<ContactResult>(); |
4302 | m_stats[ODEOtherCollisionFrameMsStatName] = 0; | 2762 | if (geom == IntPtr.Zero) |
4303 | m_stats[ODECollisionNotificationFrameMsStatName] = 0; | 2763 | return new List<ContactResult>(); |
4304 | m_stats[ODEAvatarContactsStatsName] = 0; | 2764 | |
4305 | m_stats[ODEPrimContactsStatName] = 0; | 2765 | ContactResult[] ourResults = null; |
4306 | m_stats[ODEAvatarUpdateFrameMsStatName] = 0; | 2766 | RayCallback retMethod = delegate(List<ContactResult> results) |
4307 | m_stats[ODEPrimUpdateFrameMsStatName] = 0; | 2767 | { |
2768 | ourResults = new ContactResult[results.Count]; | ||
2769 | results.CopyTo(ourResults, 0); | ||
2770 | }; | ||
2771 | int waitTime = 0; | ||
2772 | m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod); | ||
2773 | while (ourResults == null && waitTime < 1000) | ||
2774 | { | ||
2775 | Thread.Sleep(1); | ||
2776 | waitTime++; | ||
2777 | } | ||
2778 | if (ourResults == null) | ||
2779 | return new List<ContactResult>(); | ||
2780 | return new List<ContactResult>(ourResults); | ||
2781 | } | ||
2782 | return new List<ContactResult>(); | ||
4308 | } | 2783 | } |
4309 | } | 2784 | } |
4310 | } \ No newline at end of file | 2785 | } |