aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
diff options
context:
space:
mode:
authorUbitUmarov2012-10-12 00:36:01 +0100
committerUbitUmarov2012-10-12 00:36:01 +0100
commitd5cfe1c0be89f5d212b77c4b0e750ef409ba09bd (patch)
tree863af77e3f350ee71e4c23569c068fb4f7513c94 /OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
parentnormalize quaternion.Slerp outputs (diff)
downloadopensim-SC-d5cfe1c0be89f5d212b77c4b0e750ef409ba09bd.zip
opensim-SC-d5cfe1c0be89f5d212b77c4b0e750ef409ba09bd.tar.gz
opensim-SC-d5cfe1c0be89f5d212b77c4b0e750ef409ba09bd.tar.bz2
opensim-SC-d5cfe1c0be89f5d212b77c4b0e750ef409ba09bd.tar.xz
remove some more debug spam on ode
Diffstat (limited to 'OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs')
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs5346
1 files changed, 2395 insertions, 2951 deletions
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
index eaee950..eaf0d0a 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
@@ -25,11 +25,6 @@
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
33/* 28/*
34 * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces 29 * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces
35 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: 30 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
@@ -53,251 +48,250 @@ using System.Runtime.InteropServices;
53using System.Threading; 48using System.Threading;
54using log4net; 49using log4net;
55using OpenMetaverse; 50using OpenMetaverse;
56using OdeAPI; 51using Ode.NET;
57using OpenSim.Framework; 52using OpenSim.Framework;
58using OpenSim.Region.Physics.Manager; 53using OpenSim.Region.Physics.Manager;
59 54
60namespace OpenSim.Region.Physics.OdePlugin 55namespace OpenSim.Region.Physics.OdePlugin
61{ 56{
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>
62 public class OdePrim : PhysicsActor 60 public class OdePrim : PhysicsActor
63 { 61 {
64 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); 62 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
65 63
66 private bool m_isphysical; 64 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
72 65
73 protected bool m_building; 66 public int ExpectedCollisionContacts { get { return m_expectedCollisionContacts; } }
74 protected bool m_forcePosOrRotation; 67 private int m_expectedCollisionContacts = 0;
75 private bool m_iscolliding;
76 68
77 internal bool m_isSelected; 69 /// <summary>
78 private bool m_delaySelect; 70 /// Is this prim subject to physics? Even if not, it's still solid for collision purposes.
79 private bool m_lastdoneSelected; 71 /// </summary>
80 internal bool m_outbounds; 72 public override bool IsPhysical
81 73 {
82 private Quaternion m_lastorientation; 74 get { return m_isphysical; }
83 private Quaternion _orientation; 75 set
76 {
77 m_isphysical = value;
78 if (!m_isphysical) // Zero the remembered last velocity
79 m_lastVelocity = Vector3.Zero;
80 }
81 }
84 82
85 private Vector3 _position; 83 private Vector3 _position;
86 private Vector3 _velocity; 84 private Vector3 _velocity;
87 private Vector3 _torque; 85 private Vector3 _torque;
88 private Vector3 m_lastVelocity; 86 private Vector3 m_lastVelocity;
89 private Vector3 m_lastposition; 87 private Vector3 m_lastposition;
88 private Quaternion m_lastorientation = new Quaternion();
90 private Vector3 m_rotationalVelocity; 89 private Vector3 m_rotationalVelocity;
91 private Vector3 _size; 90 private Vector3 _size;
92 private Vector3 _acceleration; 91 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;
93 private Vector3 m_angularlock = Vector3.One; 99 private Vector3 m_angularlock = Vector3.One;
94 private IntPtr Amotor; 100 private Vector3 m_taintAngularLock = Vector3.One;
101 private IntPtr Amotor = IntPtr.Zero;
95 102
96 private Vector3 m_force; 103 private object m_assetsLock = new object();
97 private Vector3 m_forceacc; 104 private bool m_assetFailed = false;
98 private Vector3 m_angularForceacc;
99
100 private float m_invTimeStep;
101 private float m_timeStep;
102 105
103 private Vector3 m_PIDTarget; 106 private Vector3 m_PIDTarget;
104 private float m_PIDTau; 107 private float m_PIDTau;
108 private float PID_D = 35f;
109 private float PID_G = 25f;
105 private bool m_usePID; 110 private bool m_usePID;
106 111
112 // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
113 // and are for non-VEHICLES only.
114
107 private float m_PIDHoverHeight; 115 private float m_PIDHoverHeight;
108 private float m_PIDHoverTau; 116 private float m_PIDHoverTau;
109 private bool m_useHoverPID; 117 private bool m_useHoverPID;
110 private PIDHoverType m_PIDHoverType; 118 private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
111 private float m_targetHoverHeight; 119 private float m_targetHoverHeight;
112 private float m_groundHeight; 120 private float m_groundHeight;
113 private float m_waterHeight; 121 private float m_waterHeight;
114 private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. 122 private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
115 123
116 private int body_autodisable_frames; 124 // private float m_tensor = 5f;
117 public int bodydisablecontrol; 125 private int body_autodisable_frames = 20;
118
119 126
120 // Default we're a Geometry
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 127
125 private const CollisionCategories m_default_collisionFlagsPhysical = (CollisionCategories.Geom | 128 private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
126 CollisionCategories.Character | 129 | CollisionCategories.Space
127 CollisionCategories.Land | 130 | CollisionCategories.Body
128 CollisionCategories.VolumeDtc); 131 | CollisionCategories.Character
129 132 );
130// private bool m_collidesLand = true; 133 private bool m_taintshape;
134 private bool m_taintPhysics;
135 private bool m_collidesLand = true;
131 private bool m_collidesWater; 136 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 137
138 // Default we're a Geometry
139 private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
136 140
137 // Default, Collide with Other Geometries, spaces and Bodies 141 // Default, Collide with Other Geometries, spaces and Bodies
138 private CollisionCategories m_collisionFlags = m_default_collisionFlagsNotPhysical; 142 private CollisionCategories m_collisionFlags = m_default_collisionFlags;
139 143
140 public bool m_disabled; 144 public bool m_taintremove { get; private set; }
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; }
141 150
142 private uint m_localID; 151 private bool m_taintforce = false;
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>();
143 156
144 private IMesh m_mesh;
145 private object m_meshlock = new object();
146 private PrimitiveBaseShape _pbs; 157 private PrimitiveBaseShape _pbs;
158 private OdeScene _parent_scene;
147 159
148 private UUID? m_assetID; 160 /// <summary>
149 private MeshState m_meshState; 161 /// The physics space which contains prim geometries
150 162 /// </summary>
151 public OdeScene _parent_scene; 163 public IntPtr m_targetSpace = IntPtr.Zero;
152 164
153 /// <summary> 165 /// <summary>
154 /// The physics space which contains prim geometry 166 /// The prim geometry, used for collision detection.
155 /// </summary> 167 /// </summary>
156 public IntPtr m_targetSpace; 168 /// <remarks>
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; }
157 173
158 public IntPtr prim_geom; 174 public IntPtr _triMeshData { get; private set; }
159 public IntPtr _triMeshData;
160 175
176 private IntPtr _linkJointGroup = IntPtr.Zero;
161 private PhysicsActor _parent; 177 private PhysicsActor _parent;
178 private PhysicsActor m_taintparent;
162 179
163 private List<OdePrim> childrenPrim = new List<OdePrim>(); 180 private List<OdePrim> childrenPrim = new List<OdePrim>();
164 181
165 public float m_collisionscore; 182 private bool iscolliding;
166 private int m_colliderfilter = 0; 183 private bool m_isSelected;
167
168 public IntPtr collide_geom; // for objects: geom if single prim space it linkset
169
170 private float m_density;
171 private byte m_shapetype;
172 public bool _zeroFlag;
173 private bool m_lastUpdateSent;
174 184
175 public IntPtr Body; 185 internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
176 186
177 private Vector3 _target_velocity; 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;
178 193
179 public Vector3 m_OBBOffset; 194 public bool outofBounds { get; private set; }
180 public Vector3 m_OBB; 195 private float m_density = 10.000006836f; // Aluminum g/cm3;
181 public float primOOBradiusSQ;
182 196
183 private bool m_hasOBB = true; 197 public bool _zeroFlag { get; private set; }
198 private bool m_lastUpdateSent;
184 199
185 private float m_physCost; 200 public IntPtr Body = IntPtr.Zero;
186 private float m_streamCost; 201 private Vector3 _target_velocity;
202 private d.Mass pMass;
187 203
188 public d.Mass primdMass; // prim inertia information on it's own referencial 204 private int m_eventsubscription;
189 float primMass; // prim own mass 205 private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
190 float primVolume; // prim own volume;
191 float _mass; // object mass acording to case
192 206
193 public int givefakepos; 207 /// <summary>
194 private Vector3 fakepos; 208 /// Signal whether there were collisions on the previous frame, so we know if we need to send the
195 public int givefakeori; 209 /// empty CollisionEventsThisFrame to the prim so that it can detect the end of a collision.
196 private Quaternion fakeori; 210 /// </summary>
211 /// <remarks>
212 /// This is probably a temporary measure, pending storing this information consistently in CollisionEventUpdate itself.
213 /// </remarks>
214 private bool m_collisionsOnPreviousFrame;
197 215
198 private int m_eventsubscription; 216 private IntPtr m_linkJoint = IntPtr.Zero;
199 private int m_cureventsubscription;
200 private CollisionEventUpdate CollisionEventsThisFrame = null;
201 private bool SentEmptyCollisionsEvent;
202 217
203 public volatile bool childPrim; 218 internal volatile bool childPrim;
204 219
205 public ODEDynamics m_vehicle; 220 private ODEDynamics m_vehicle;
206 221
207 internal int m_material = (int)Material.Wood; 222 internal int m_material = (int)Material.Wood;
208 private float mu;
209 private float bounce;
210 223
211 /// <summary> 224 public OdePrim(
212 /// Is this prim subject to physics? Even if not, it's still solid for collision purposes. 225 String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
213 /// </summary> 226 Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical)
214 public override bool IsPhysical // this is not reliable for internal use
215 { 227 {
216 get { return m_fakeisphysical; } 228 Name = primName;
217 set 229 m_vehicle = new ODEDynamics();
218 { 230 //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
219 m_fakeisphysical = value; // we show imediatly to outside that we changed physical
220 // and also to stop imediatly some updates
221 // but real change will only happen in taintprocessing
222
223 if (!value) // Zero the remembered last velocity
224 m_lastVelocity = Vector3.Zero;
225 AddChange(changes.Physical, value);
226 }
227 }
228 231
229 public override bool IsVolumeDtc 232 if (!pos.IsFinite())
230 {
231 get { return m_fakeisVolumeDetect; }
232 set
233 { 233 {
234 m_fakeisVolumeDetect = value; 234 pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
235 AddChange(changes.VolumeDtc, value); 235 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
236 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name);
236 } 237 }
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;
238 245
239 public override bool Phantom // this is not reliable for internal use 246 prim_geom = IntPtr.Zero;
240 {
241 get { return m_fakeisphantom; }
242 set
243 {
244 m_fakeisphantom = value;
245 AddChange(changes.Phantom, value);
246 }
247 }
248 247
249 public override bool Building // this is not reliable for internal use 248 if (!pos.IsFinite())
250 {
251 get { return m_building; }
252 set
253 { 249 {
254 if (value) 250 size = new Vector3(0.5f, 0.5f, 0.5f);
255 m_building = true; 251 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name);
256 AddChange(changes.building, value);
257 } 252 }
258 }
259 253
260 public override void getContactData(ref ContactData cdata) 254 if (size.X <= 0) size.X = 0.01f;
261 { 255 if (size.Y <= 0) size.Y = 0.01f;
262 cdata.mu = mu; 256 if (size.Z <= 0) size.Z = 0.01f;
263 cdata.bounce = bounce;
264 257
265 // cdata.softcolide = m_softcolide; 258 _size = size;
266 cdata.softcolide = false; 259 m_taintsize = _size;
267 260
268 if (m_isphysical) 261 if (!QuaternionIsFinite(rotation))
269 { 262 {
270 ODEDynamics veh; 263 rotation = Quaternion.Identity;
271 if (_parent != null) 264 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name);
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;
279 } 265 }
280 }
281 266
282 public override float PhysicsCost 267 _orientation = rotation;
283 { 268 m_taintrot = _orientation;
284 get 269 _pbs = pbs;
270
271 _parent_scene = parent_scene;
272 m_targetSpace = (IntPtr)0;
273
274 if (pos.Z < 0)
285 { 275 {
286 return m_physCost; 276 IsPhysical = false;
287 } 277 }
288 } 278 else
289
290 public override float StreamCost
291 {
292 get
293 { 279 {
294 return m_streamCost; 280 IsPhysical = pisPhysical;
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;
295 } 285 }
286
287 m_taintadd = true;
288 m_assetFailed = false;
289 _parent_scene.AddPhysicsActorTaint(this);
296 } 290 }
297 291
298 public override int PhysicsActorType 292 public override int PhysicsActorType
299 { 293 {
300 get { return (int)ActorTypes.Prim; } 294 get { return (int) ActorTypes.Prim; }
301 set { return; } 295 set { return; }
302 } 296 }
303 297
@@ -307,23 +301,6 @@ namespace OpenSim.Region.Physics.OdePlugin
307 set { return; } 301 set { return; }
308 } 302 }
309 303
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
327 public override bool Grabbed 304 public override bool Grabbed
328 { 305 {
329 set { return; } 306 set { return; }
@@ -333,3068 +310,2383 @@ namespace OpenSim.Region.Physics.OdePlugin
333 { 310 {
334 set 311 set
335 { 312 {
336 if (value) 313 // This only makes the object not collidable if the object
337 m_isSelected = value; // if true set imediatly to stop moves etc 314 // is physical or the object is modified somehow *IN THE FUTURE*
338 AddChange(changes.Selected, value); 315 // without this, if an avatar selects prim, they can walk right
339 } 316 // through it while it's selected
340 } 317 m_collisionscore = 0;
341
342 public override bool Flying
343 {
344 // no flying prims for you
345 get { return false; }
346 set { }
347 }
348 318
349 public override bool IsColliding 319 if ((IsPhysical && !_zeroFlag) || !value)
350 {
351 get { return m_iscolliding; }
352 set
353 {
354 if (value)
355 { 320 {
356 m_colliderfilter += 2; 321 m_taintselected = value;
357 if (m_colliderfilter > 2) 322 _parent_scene.AddPhysicsActorTaint(this);
358 m_colliderfilter = 2;
359 } 323 }
360 else 324 else
361 { 325 {
362 m_colliderfilter--; 326 m_taintselected = value;
363 if (m_colliderfilter < 0) 327 m_isSelected = value;
364 m_colliderfilter = 0;
365 } 328 }
366 329
367 if (m_colliderfilter == 0) 330 if (m_isSelected)
368 m_iscolliding = false; 331 disableBodySoft();
369 else
370 m_iscolliding = true;
371 } 332 }
372 } 333 }
373 334
374 public override bool CollidingGround 335 /// <summary>
375 { 336 /// Set a new geometry for this prim.
376 get { return false; } 337 /// </summary>
377 set { return; } 338 /// <param name="geom"></param>
378 } 339 private void SetGeom(IntPtr geom)
379
380 public override bool CollidingObj
381 {
382 get { return false; }
383 set { return; }
384 }
385
386
387 public override bool ThrottleUpdates {get;set;}
388
389 public override bool Stopped
390 { 340 {
391 get { return _zeroFlag; } 341 prim_geom = geom;
392 } 342//Console.WriteLine("SetGeom to " + prim_geom + " for " + Name);
393 343
394 public override Vector3 Position 344 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
395 { 345 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
396 get
397 {
398 if (givefakepos > 0)
399 return fakepos;
400 else
401 return _position;
402 }
403 346
404 set 347 _parent_scene.geom_name_map[prim_geom] = Name;
405 { 348 _parent_scene.actor_name_map[prim_geom] = this;
406 fakepos = value;
407 givefakepos++;
408 AddChange(changes.Position, value);
409 }
410 }
411 349
412 public override Vector3 Size 350 if (childPrim)
413 {
414 get { return _size; }
415 set
416 { 351 {
417 if (value.IsFinite()) 352 if (_parent != null && _parent is OdePrim)
418 {
419 _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, value, m_shapetype);
420 }
421 else
422 { 353 {
423 m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name); 354 OdePrim parent = (OdePrim)_parent;
355//Console.WriteLine("SetGeom calls ChildSetGeom");
356 parent.ChildSetGeom(this);
424 } 357 }
425 } 358 }
359 //m_log.Warn("Setting Geom to: " + prim_geom);
426 } 360 }
427 361
428 public override float Mass 362 private void enableBodySoft()
429 {
430 get { return primMass; }
431 }
432
433 public override Vector3 Force
434 { 363 {
435 get { return m_force; } 364 if (!childPrim)
436 set
437 { 365 {
438 if (value.IsFinite()) 366 if (IsPhysical && Body != IntPtr.Zero)
439 {
440 AddChange(changes.Force, value);
441 }
442 else
443 { 367 {
444 m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name); 368 d.BodyEnable(Body);
369 if (m_vehicle.Type != Vehicle.TYPE_NONE)
370 m_vehicle.Enable(Body, _parent_scene);
445 } 371 }
372
373 m_disabled = false;
446 } 374 }
447 } 375 }
448 376
449 public override void SetVolumeDetect(int param) 377 private void disableBodySoft()
450 { 378 {
451 m_fakeisVolumeDetect = (param != 0); 379 m_disabled = true;
452 AddChange(changes.VolumeDtc, m_fakeisVolumeDetect);
453 }
454 380
455 public override Vector3 GeometricCenter 381 if (IsPhysical && Body != IntPtr.Zero)
456 {
457 // this is not real geometric center but a average of positions relative to root prim acording to
458 // http://wiki.secondlife.com/wiki/llGetGeometricCenter
459 // ignoring tortured prims details since sl also seems to ignore
460 // so no real use in doing it on physics
461 get
462 { 382 {
463 return Vector3.Zero; 383 d.BodyDisable(Body);
464 } 384 }
465 } 385 }
466 386
467 public override Vector3 CenterOfMass 387 /// <summary>
388 /// Make a prim subject to physics.
389 /// </summary>
390 private void enableBody()
468 { 391 {
469 get 392 // Don't enable this body if we're a child prim
393 // this should be taken care of in the parent function not here
394 if (!childPrim)
470 { 395 {
471 lock (_parent_scene.OdeLock) 396 // Sets the geom to a body
472 { 397 Body = d.BodyCreate(_parent_scene.world);
473 d.Vector3 dtmp;
474 if (!childPrim && Body != IntPtr.Zero)
475 {
476 dtmp = d.BodyGetPosition(Body);
477 return new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
478 }
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;
500 398
501 float m; 399 setMass();
502 400 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
503 foreach (OdePrim prm in childrenPrim) 401 d.Quaternion myrot = new d.Quaternion();
504 { 402 myrot.X = _orientation.X;
505 m = prm._mass; 403 myrot.Y = _orientation.Y;
506 Ptot += prm.CenterOfMass * m; 404 myrot.Z = _orientation.Z;
507 tmass += m; 405 myrot.W = _orientation.W;
508 } 406 d.BodySetQuaternion(Body, ref myrot);
407 d.GeomSetBody(prim_geom, Body);
408 m_collisionCategories |= CollisionCategories.Body;
409 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
509 410
510 if (tmass == 0) 411 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
511 tmass = 0; 412 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
512 else
513 tmass = 1.0f / tmass;
514 413
515 Ptot *= tmass; 414 d.BodySetAutoDisableFlag(Body, true);
516 return Ptot; 415 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
517*/ 416
518 } 417 // disconnect from world gravity so we can apply buoyancy
519 else 418 d.BodySetGravityMode (Body, false);
520 return _position;
521 }
522 }
523 }
524 419
525 public override Vector3 OOBsize 420 m_interpenetrationcount = 0;
526 { 421 m_collisionscore = 0;
527 get 422 m_disabled = false;
528 {
529 return m_OBB;
530 }
531 }
532 423
533 public override Vector3 OOBoffset 424 // The body doesn't already have a finite rotation mode set here
534 { 425 if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0.0f)) && _parent == null)
535 get
536 { 426 {
537 return m_OBBOffset; 427 createAMotor(m_angularlock);
538 } 428 }
539 } 429 if (m_vehicle.Type != Vehicle.TYPE_NONE)
540
541 public override float OOBRadiusSQ
542 {
543 get
544 { 430 {
545 return primOOBradiusSQ; 431 m_vehicle.Enable(Body, _parent_scene);
546 } 432 }
547 }
548 433
549 public override PrimitiveBaseShape Shape 434 _parent_scene.ActivatePrim(this);
550 {
551 set
552 {
553// AddChange(changes.Shape, value);
554 _parent_scene.m_meshWorker.ChangeActorPhysRep(this, value, _size, m_shapetype);
555 } 435 }
556 } 436 }
557 437
558 public override byte PhysicsShapeType 438 #region Mass Calculation
559 {
560 get
561 {
562 return m_shapetype;
563 }
564 set
565 {
566 m_shapetype = value;
567 _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, _size, value);
568 }
569 }
570 439
571 public override Vector3 Velocity 440 private float CalculateMass()
572 { 441 {
573 get 442 float volume = _size.X * _size.Y * _size.Z; // default
574 { 443 float tmp;
575 if (_zeroFlag) 444
576 return Vector3.Zero; 445 float returnMass = 0;
577 return _velocity; 446 float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
578 } 447 float hollowVolume = hollowAmount * hollowAmount;
579 set 448
449 switch (_pbs.ProfileShape)
580 { 450 {
581 if (value.IsFinite()) 451 case ProfileShape.Square:
582 { 452 // default box
583 AddChange(changes.Velocity, value);
584 }
585 else
586 {
587 m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name);
588 }
589 453
590 } 454 if (_pbs.PathCurve == (byte)Extrusion.Straight)
591 } 455 {
456 if (hollowAmount > 0.0)
457 {
458 switch (_pbs.HollowShape)
459 {
460 case HollowShape.Square:
461 case HollowShape.Same:
462 break;
592 463
593 public override Vector3 Torque 464 case HollowShape.Circle:
594 {
595 get
596 {
597 if (!IsPhysical || Body == IntPtr.Zero)
598 return Vector3.Zero;
599 465
600 return _torque; 466 hollowVolume *= 0.78539816339f;
601 } 467 break;
602 468
603 set 469 case HollowShape.Triangle:
604 {
605 if (value.IsFinite())
606 {
607 AddChange(changes.Torque, value);
608 }
609 else
610 {
611 m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name);
612 }
613 }
614 }
615 470
616 public override float CollisionScore 471 hollowVolume *= (0.5f * .5f);
617 { 472 break;
618 get { return m_collisionscore; }
619 set { m_collisionscore = value; }
620 }
621 473
622 public override bool Kinematic 474 default:
623 { 475 hollowVolume = 0;
624 get { return false; } 476 break;
625 set { } 477 }
626 } 478 volume *= (1.0f - hollowVolume);
479 }
480 }
627 481
628 public override Quaternion Orientation 482 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
629 { 483 {
630 get 484 //a tube
631 {
632 if (givefakeori > 0)
633 return fakeori;
634 else
635 485
636 return _orientation; 486 volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
637 } 487 tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY);
638 set 488 volume -= volume*tmp*tmp;
639 { 489
640 if (QuaternionIsFinite(value)) 490 if (hollowAmount > 0.0)
641 { 491 {
642 fakeori = value; 492 hollowVolume *= hollowAmount;
643 givefakeori++; 493
644// Debug 494 switch (_pbs.HollowShape)
645 float qlen = value.Length(); 495 {
646 if (value.Length() > 1.01f || qlen <0.99) 496 case HollowShape.Square:
647 m_log.WarnFormat("[PHYSICS]: Got nonnorm quaternion Orientation from Scene in Object {0} norm {1}", Name, qlen); 497 case HollowShape.Same:
648// 498 break;
649 value.Normalize(); 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 }
650 514
651 AddChange(changes.Orientation, value); 515 break;
652 }
653 else
654 m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name);
655 516
656 } 517 case ProfileShape.Circle:
657 }
658 518
659 public override Vector3 Acceleration 519 if (_pbs.PathCurve == (byte)Extrusion.Straight)
660 { 520 {
661 get { return _acceleration; } 521 volume *= 0.78539816339f; // elipse base
662 set { }
663 }
664 522
665 public override Vector3 RotationalVelocity 523 if (hollowAmount > 0.0)
666 { 524 {
667 get 525 switch (_pbs.HollowShape)
668 { 526 {
669 Vector3 pv = Vector3.Zero; 527 case HollowShape.Same:
670 if (_zeroFlag) 528 case HollowShape.Circle:
671 return pv; 529 break;
672 530
673 if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) 531 case HollowShape.Square:
674 return pv; 532 hollowVolume *= 0.5f * 2.5984480504799f;
533 break;
675 534
676 return m_rotationalVelocity; 535 case HollowShape.Triangle:
677 } 536 hollowVolume *= .5f * 1.27323954473516f;
678 set 537 break;
679 {
680 if (value.IsFinite())
681 {
682 AddChange(changes.AngVelocity, value);
683 }
684 else
685 {
686 m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name);
687 }
688 }
689 }
690 538
691 public override float Buoyancy 539 default:
692 { 540 hollowVolume = 0;
693 get { return m_buoyancy; } 541 break;
694 set 542 }
695 { 543 volume *= (1.0f - hollowVolume);
696 AddChange(changes.Buoyancy,value); 544 }
697 } 545 }
698 }
699 546
700 public override bool FloatOnWater 547 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
701 { 548 {
702 set 549 volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
703 { 550 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
704 AddChange(changes.CollidesWater, value); 551 volume *= (1.0f - tmp * tmp);
705 } 552
706 } 553 if (hollowAmount > 0.0)
554 {
707 555
708 public override Vector3 PIDTarget 556 // calculate the hollow volume by it's shape compared to the prim shape
709 { 557 hollowVolume *= hollowAmount;
710 set
711 {
712 if (value.IsFinite())
713 {
714 AddChange(changes.PIDTarget,value);
715 }
716 else
717 m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name);
718 }
719 }
720 558
721 public override bool PIDActive 559 switch (_pbs.HollowShape)
722 { 560 {
723 set 561 case HollowShape.Same:
724 { 562 case HollowShape.Circle:
725 AddChange(changes.PIDActive,value); 563 break;
726 }
727 }
728 564
729 public override float PIDTau 565 case HollowShape.Square:
730 { 566 hollowVolume *= 0.5f * 2.5984480504799f;
731 set 567 break;
732 {
733 float tmp = 0;
734 if (value > 0)
735 {
736 float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep);
737 if (value < mint)
738 tmp = mint;
739 else
740 tmp = value;
741 }
742 AddChange(changes.PIDTau,tmp);
743 }
744 }
745 568
746 public override float PIDHoverHeight 569 case HollowShape.Triangle:
747 { 570 hollowVolume *= .5f * 1.27323954473516f;
748 set 571 break;
749 {
750 AddChange(changes.PIDHoverHeight,value);
751 }
752 }
753 public override bool PIDHoverActive
754 {
755 set
756 {
757 AddChange(changes.PIDHoverActive, value);
758 }
759 }
760 572
761 public override PIDHoverType PIDHoverType 573 default:
762 { 574 hollowVolume = 0;
763 set 575 break;
764 { 576 }
765 AddChange(changes.PIDHoverType,value); 577 volume *= (1.0f - hollowVolume);
766 } 578 }
767 } 579 }
580 break;
768 581
769 public override float PIDHoverTau 582 case ProfileShape.HalfCircle:
770 { 583 if (_pbs.PathCurve == (byte)Extrusion.Curve1)
771 set 584 {
772 { 585 volume *= 0.52359877559829887307710723054658f;
773 float tmp =0; 586 }
774 if (value > 0) 587 break;
775 {
776 float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep);
777 if (value < mint)
778 tmp = mint;
779 else
780 tmp = value;
781 }
782 AddChange(changes.PIDHoverTau, tmp);
783 }
784 }
785 588
786 public override Quaternion APIDTarget { set { return; } } 589 case ProfileShape.EquilateralTriangle:
787 590
788 public override bool APIDActive { set { return; } } 591 if (_pbs.PathCurve == (byte)Extrusion.Straight)
592 {
593 volume *= 0.32475953f;
789 594
790 public override float APIDStrength { set { return; } } 595 if (hollowAmount > 0.0)
596 {
791 597
792 public override float APIDDamping { set { return; } } 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;
793 605
794 public override int VehicleType 606 case HollowShape.Square:
795 { 607 hollowVolume *= 0.499849f * 3.07920140172638f;
796 // we may need to put a fake on this 608 break;
797 get
798 {
799 if (m_vehicle == null)
800 return (int)Vehicle.TYPE_NONE;
801 else
802 return (int)m_vehicle.Type;
803 }
804 set
805 {
806 AddChange(changes.VehicleType, value);
807 }
808 }
809 609
810 public override void VehicleFloatParam(int param, float value) 610 case HollowShape.Circle:
811 { 611 // Hollow shape is a perfect cyllinder in respect to the cube's scale
812 strVehicleFloatParam fp = new strVehicleFloatParam(); 612 // Cyllinder hollow volume calculation
813 fp.param = param;
814 fp.value = value;
815 AddChange(changes.VehicleFloatParam, fp);
816 }
817 613
818 public override void VehicleVectorParam(int param, Vector3 value) 614 hollowVolume *= 0.1963495f * 3.07920140172638f;
819 { 615 break;
820 strVehicleVectorParam fp = new strVehicleVectorParam();
821 fp.param = param;
822 fp.value = value;
823 AddChange(changes.VehicleVectorParam, fp);
824 }
825 616
826 public override void VehicleRotationParam(int param, Quaternion value) 617 default:
827 { 618 hollowVolume = 0;
828 strVehicleQuatParam fp = new strVehicleQuatParam(); 619 break;
829 fp.param = param; 620 }
830 fp.value = value; 621 volume *= (1.0f - hollowVolume);
831 AddChange(changes.VehicleRotationParam, fp); 622 }
832 } 623 }
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);
833 630
834 public override void VehicleFlags(int param, bool value) 631 if (hollowAmount > 0.0)
835 { 632 {
836 strVehicleBoolParam bp = new strVehicleBoolParam();
837 bp.param = param;
838 bp.value = value;
839 AddChange(changes.VehicleFlags, bp);
840 }
841 633
842 public override void SetVehicle(object vdata) 634 hollowVolume *= hollowAmount;
843 {
844 AddChange(changes.SetVehicle, vdata);
845 }
846 public void SetAcceleration(Vector3 accel)
847 {
848 _acceleration = accel;
849 }
850 635
851 public override void AddForce(Vector3 force, bool pushforce) 636 switch (_pbs.HollowShape)
852 { 637 {
853 if (force.IsFinite()) 638 case HollowShape.Same:
854 { 639 case HollowShape.Triangle:
855 if(pushforce) 640 hollowVolume *= .25f;
856 AddChange(changes.AddForce, force); 641 break;
857 else // a impulse
858 AddChange(changes.AddForce, force * m_invTimeStep);
859 }
860 else
861 {
862 m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name);
863 }
864 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
865 }
866 642
867 public override void AddAngularForce(Vector3 force, bool pushforce) 643 case HollowShape.Square:
868 { 644 hollowVolume *= 0.499849f * 3.07920140172638f;
869 if (force.IsFinite()) 645 break;
646
647 case HollowShape.Circle:
648
649 hollowVolume *= 0.1963495f * 3.07920140172638f;
650 break;
651
652 default:
653 hollowVolume = 0;
654 break;
655 }
656 volume *= (1.0f - hollowVolume);
657 }
658 }
659 break;
660
661 default:
662 break;
663 }
664
665 float taperX1;
666 float taperY1;
667 float taperX;
668 float taperY;
669 float pathBegin;
670 float pathEnd;
671 float profileBegin;
672 float profileEnd;
673
674 if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible)
870 { 675 {
871// if(pushforce) for now applyrotationimpulse seems more happy applied as a force 676 taperX1 = _pbs.PathScaleX * 0.01f;
872 AddChange(changes.AddAngForce, force); 677 if (taperX1 > 1.0f)
873// else // a impulse 678 taperX1 = 2.0f - taperX1;
874// AddChange(changes.AddAngForce, force * m_invTimeStep); 679 taperX = 1.0f - taperX1;
680
681 taperY1 = _pbs.PathScaleY * 0.01f;
682 if (taperY1 > 1.0f)
683 taperY1 = 2.0f - taperY1;
684 taperY = 1.0f - taperY1;
875 } 685 }
876 else 686 else
877 { 687 {
878 m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name); 688 taperX = _pbs.PathTaperX * 0.01f;
689 if (taperX < 0.0f)
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;
879 } 697 }
880 }
881 698
882 public override void CrossingFailure() 699 volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY);
883 {
884 if (m_outbounds)
885 {
886 _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);
887 _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);
888 _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f);
889 700
890 m_lastposition = _position; 701 pathBegin = (float)_pbs.PathBegin * 2.0e-5f;
891 _velocity.X = 0; 702 pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f;
892 _velocity.Y = 0; 703 volume *= (pathEnd - pathBegin);
893 _velocity.Z = 0;
894 704
895 m_lastVelocity = _velocity; 705// this is crude aproximation
896 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) 706 profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f;
897 m_vehicle.Stop(); 707 profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f;
708 volume *= (profileEnd - profileBegin);
898 709
899 if(Body != IntPtr.Zero) 710 returnMass = m_density * volume;
900 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
901 if (prim_geom != IntPtr.Zero)
902 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
903 711
904 m_outbounds = false; 712 if (returnMass <= 0)
905 changeDisable(false); 713 returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
906 base.RequestPhysicsterseUpdate(); 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 {
721 if (childrenPrim.Count > 0)
722 {
723 HasChildPrim = true;
724 }
907 } 725 }
908 }
909 726
910 public override void SetMomentum(Vector3 momentum) 727 if (HasChildPrim)
911 { 728 {
912 } 729 OdePrim[] childPrimArr = new OdePrim[0];
913 730
914 public override void SetMaterial(int pMaterial) 731 lock (childrenPrim)
915 { 732 childPrimArr = childrenPrim.ToArray();
916 m_material = pMaterial;
917 mu = _parent_scene.m_materialContactsData[pMaterial].mu;
918 bounce = _parent_scene.m_materialContactsData[pMaterial].bounce;
919 }
920 733
921 public void setPrimForRemoval() 734 for (int i = 0; i < childPrimArr.Length; i++)
922 { 735 {
923 AddChange(changes.Remove, null); 736 if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove)
924 } 737 returnMass += childPrimArr[i].CalculateMass();
738 // failsafe, this shouldn't happen but with OpenSim, you never know :)
739 if (i > 256)
740 break;
741 }
742 }
925 743
926 public override void link(PhysicsActor obj) 744 if (returnMass > _parent_scene.maximumMassObject)
927 { 745 returnMass = _parent_scene.maximumMassObject;
928 AddChange(changes.Link, obj);
929 }
930 746
931 public override void delink() 747 return returnMass;
932 {
933 AddChange(changes.DeLink, null);
934 } 748 }
935 749
936 public override void LockAngularMotion(Vector3 axis) 750 #endregion
751
752 private void setMass()
937 { 753 {
938 // reverse the zero/non zero values for ODE. 754 if (Body != (IntPtr) 0)
939 if (axis.IsFinite())
940 {
941 axis.X = (axis.X > 0) ? 1f : 0f;
942 axis.Y = (axis.Y > 0) ? 1f : 0f;
943 axis.Z = (axis.Z > 0) ? 1f : 0f;
944// m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
945 AddChange(changes.AngLock, axis);
946 }
947 else
948 { 755 {
949 m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name); 756 float newmass = CalculateMass();
950 }
951 }
952 757
953 public override void SubscribeEvents(int ms) 758 //m_log.Info("[PHYSICS]: New Mass: " + newmass.ToString());
954 {
955 m_eventsubscription = ms;
956 m_cureventsubscription = 0;
957 if (CollisionEventsThisFrame == null)
958 CollisionEventsThisFrame = new CollisionEventUpdate();
959 SentEmptyCollisionsEvent = false;
960 }
961 759
962 public override void UnSubscribeEvents() 760 d.MassSetBoxTotal(out pMass, newmass, _size.X, _size.Y, _size.Z);
963 { 761 d.BodySetMass(Body, ref pMass);
964 if (CollisionEventsThisFrame != null)
965 {
966 CollisionEventsThisFrame.Clear();
967 CollisionEventsThisFrame = null;
968 } 762 }
969 m_eventsubscription = 0;
970 _parent_scene.RemoveCollisionEventReporting(this);
971 } 763 }
972 764
973 public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact) 765 /// <summary>
974 { 766 /// Stop a prim from being subject to physics.
975 if (CollisionEventsThisFrame == null) 767 /// </summary>
976 CollisionEventsThisFrame = new CollisionEventUpdate(); 768 internal void disableBody()
977// if(CollisionEventsThisFrame.Count < 32)
978 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
979 }
980
981 public void SendCollisions()
982 { 769 {
983 if (CollisionEventsThisFrame == null) 770 //this kills the body so things like 'mesh' can re-create it.
984 return; 771 lock (this)
985
986 if (m_cureventsubscription < m_eventsubscription)
987 return;
988
989 m_cureventsubscription = 0;
990
991 int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count;
992
993 if (!SentEmptyCollisionsEvent || ncolisions > 0)
994 { 772 {
995 base.SendCollisionUpdate(CollisionEventsThisFrame); 773 if (!childPrim)
996
997 if (ncolisions == 0)
998 { 774 {
999 SentEmptyCollisionsEvent = true; 775 if (Body != IntPtr.Zero)
1000 _parent_scene.RemoveCollisionEventReporting(this); 776 {
777 _parent_scene.DeactivatePrim(this);
778 m_collisionCategories &= ~CollisionCategories.Body;
779 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
780
781 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
782 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
783
784 d.BodyDestroy(Body);
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 }
1001 } 798 }
1002 else 799 else
1003 { 800 {
1004 SentEmptyCollisionsEvent = false; 801 _parent_scene.DeactivatePrim(this);
1005 CollisionEventsThisFrame.Clear(); 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
809 Body = IntPtr.Zero;
1006 } 810 }
1007 } 811 }
1008 }
1009 812
1010 internal void AddCollisionFrameTime(int t) 813 m_disabled = true;
1011 { 814 m_collisionscore = 0;
1012 if (m_cureventsubscription < 50000)
1013 m_cureventsubscription += t;
1014 } 815 }
1015 816
1016 public override bool SubscribedEvents() 817 private static Dictionary<IMesh, IntPtr> m_MeshToTriMeshMap = new Dictionary<IMesh, IntPtr>();
1017 {
1018 if (m_eventsubscription > 0)
1019 return true;
1020 return false;
1021 }
1022 818
1023 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, 819 private void setMesh(OdeScene parent_scene, IMesh mesh)
1024 Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID)
1025 { 820 {
1026 Name = primName; 821// m_log.DebugFormat("[ODE PRIM]: Setting mesh on {0} to {1}", Name, mesh);
1027 LocalID = plocalID;
1028
1029 m_vehicle = null;
1030
1031 if (!pos.IsFinite())
1032 {
1033 pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
1034 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
1035 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name);
1036 }
1037 _position = pos;
1038 givefakepos = 0;
1039
1040 m_timeStep = parent_scene.ODE_STEPSIZE;
1041 m_invTimeStep = 1f / m_timeStep;
1042 822
1043 m_density = parent_scene.geomDefaultDensity; 823 // This sleeper is there to moderate how long it takes between
1044 body_autodisable_frames = parent_scene.bodyFramesAutoDisable; 824 // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object
1045 825
1046 prim_geom = IntPtr.Zero; 826 //Thread.Sleep(10);
1047 collide_geom = IntPtr.Zero;
1048 Body = IntPtr.Zero;
1049 827
1050 if (!size.IsFinite()) 828 //Kill Body so that mesh can re-make the geom
829 if (IsPhysical && Body != IntPtr.Zero)
1051 { 830 {
1052 size = new Vector3(0.5f, 0.5f, 0.5f); 831 if (childPrim)
1053 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name); 832 {
833 if (_parent != null)
834 {
835 OdePrim parent = (OdePrim)_parent;
836 parent.ChildDelink(this);
837 }
838 }
839 else
840 {
841 disableBody();
842 }
1054 } 843 }
1055 844
1056 if (size.X <= 0) size.X = 0.01f; 845 IntPtr vertices, indices;
1057 if (size.Y <= 0) size.Y = 0.01f; 846 int vertexCount, indexCount;
1058 if (size.Z <= 0) size.Z = 0.01f; 847 int vertexStride, triStride;
1059 848 mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
1060 _size = size; 849 mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
850 m_expectedCollisionContacts = indexCount;
851 mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
1061 852
1062 if (!QuaternionIsFinite(rotation)) 853 // We must lock here since m_MeshToTriMeshMap is static and multiple scene threads may call this method at
854 // the same time.
855 lock (m_MeshToTriMeshMap)
1063 { 856 {
1064 rotation = Quaternion.Identity; 857 if (m_MeshToTriMeshMap.ContainsKey(mesh))
1065 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name); 858 {
859 _triMeshData = m_MeshToTriMeshMap[mesh];
860 }
861 else
862 {
863 _triMeshData = d.GeomTriMeshDataCreate();
864
865 d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
866 d.GeomTriMeshDataPreprocess(_triMeshData);
867 m_MeshToTriMeshMap[mesh] = _triMeshData;
868 }
1066 } 869 }
1067 870
1068 _orientation = rotation; 871// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1069 givefakeori = 0; 872 try
1070
1071 _pbs = pbs;
1072
1073 _parent_scene = parent_scene;
1074 m_targetSpace = IntPtr.Zero;
1075
1076 if (pos.Z < 0)
1077 { 873 {
1078 m_isphysical = false; 874 SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
1079 } 875 }
1080 else 876 catch (AccessViolationException)
1081 { 877 {
1082 m_isphysical = pisPhysical; 878 m_log.ErrorFormat("[PHYSICS]: MESH LOCKED FOR {0}", Name);
879 return;
1083 } 880 }
1084 m_fakeisphysical = m_isphysical;
1085 881
1086 m_isVolumeDetect = false; 882 // if (IsPhysical && Body == (IntPtr) 0)
1087 m_fakeisVolumeDetect = false; 883 // {
884 // Recreate the body
885 // m_interpenetrationcount = 0;
886 // m_collisionscore = 0;
1088 887
1089 m_force = Vector3.Zero; 888 // enableBody();
889 // }
890 }
1090 891
1091 m_iscolliding = false; 892 internal void ProcessTaints()
1092 m_colliderfilter = 0; 893 {
1093 m_NoColide = false; 894#if SPAM
895Console.WriteLine("ZProcessTaints for " + Name);
896#endif
1094 897
1095 _triMeshData = IntPtr.Zero; 898 // This must be processed as the very first taint so that later operations have a prim_geom to work with
899 // if this is a new prim.
900 if (m_taintadd)
901 changeadd();
1096 902
1097 m_shapetype = _shapeType; 903 if (!_position.ApproxEquals(m_taintposition, 0f))
904 changemove();
1098 905
1099 m_lastdoneSelected = false; 906 if (m_taintrot != _orientation)
1100 m_isSelected = false; 907 {
1101 m_delaySelect = false; 908 if (childPrim && IsPhysical) // For physical child prim...
909 {
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
917 {
918 //Just rotate the prim
919 rotate();
920 }
921 }
922
923 if (m_taintPhysics != IsPhysical && !(m_taintparent != _parent))
924 changePhysicsStatus();
1102 925
1103 m_isphantom = pisPhantom; 926 if (!_size.ApproxEquals(m_taintsize, 0f))
1104 m_fakeisphantom = pisPhantom; 927 changesize();
1105 928
1106 mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu; 929 if (m_taintshape)
1107 bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce; 930 changeshape();
1108 931
1109 m_building = true; // control must set this to false when done 932 if (m_taintforce)
933 changeAddForce();
1110 934
1111 _parent_scene.m_meshWorker.NewActorPhysRep(this, _pbs, _size, m_shapetype); 935 if (m_taintaddangularforce)
1112 } 936 changeAddAngularForce();
1113 937
1114 private void resetCollisionAccounting() 938 if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f))
1115 { 939 changeSetTorque();
1116 m_collisionscore = 0;
1117 }
1118 940
1119 private void UpdateCollisionCatFlags() 941 if (m_taintdisable)
1120 { 942 changedisable();
1121 if(m_isphysical && m_disabled)
1122 {
1123 m_collisionCategories = 0;
1124 m_collisionFlags = 0;
1125 }
1126 943
1127 else if (m_isSelected) 944 if (m_taintselected != m_isSelected)
1128 { 945 changeSelectedStatus();
1129 m_collisionCategories = CollisionCategories.Selected;
1130 m_collisionFlags = 0;
1131 }
1132 946
1133 else if (m_isVolumeDetect) 947 if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f))
1134 { 948 changevelocity();
1135 m_collisionCategories = CollisionCategories.VolumeDtc; 949
1136 if (m_isphysical) 950 if (m_taintparent != _parent)
1137 m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character; 951 changelink();
1138 else 952
1139 m_collisionFlags = 0; 953 if (m_taintCollidesWater != m_collidesWater)
1140 } 954 changefloatonwater();
1141 else if (m_isphantom) 955
1142 { 956 if (!m_angularlock.ApproxEquals(m_taintAngularLock,0f))
1143 m_collisionCategories = CollisionCategories.Phantom; 957 changeAngularLock();
1144 if (m_isphysical)
1145 m_collisionFlags = CollisionCategories.Land;
1146 else
1147 m_collisionFlags = 0;
1148 }
1149 else
1150 {
1151 m_collisionCategories = CollisionCategories.Geom;
1152 if (m_isphysical)
1153 m_collisionFlags = m_default_collisionFlagsPhysical;
1154 else
1155 m_collisionFlags = m_default_collisionFlagsNotPhysical;
1156 }
1157 } 958 }
1158 959
1159 private void ApplyCollisionCatFlags() 960 /// <summary>
961 /// Change prim in response to an angular lock taint.
962 /// </summary>
963 private void changeAngularLock()
1160 { 964 {
1161 if (prim_geom != IntPtr.Zero) 965 // do we have a Physical object?
966 if (Body != IntPtr.Zero)
1162 { 967 {
1163 if (!childPrim && childrenPrim.Count > 0) 968 //Check that we have a Parent
969 //If we have a parent then we're not authorative here
970 if (_parent == null)
1164 { 971 {
1165 foreach (OdePrim prm in childrenPrim) 972 if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f))
1166 { 973 {
1167 if (m_isphysical && m_disabled) 974 //d.BodySetFiniteRotationMode(Body, 0);
1168 { 975 //d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z);
1169 prm.m_collisionCategories = 0; 976 createAMotor(m_taintAngularLock);
1170 prm.m_collisionFlags = 0; 977 }
1171 } 978 else
1172 else 979 {
1173 { 980 if (Amotor != IntPtr.Zero)
1174 // preserve some
1175 if (prm.m_isSelected)
1176 {
1177 prm.m_collisionCategories = CollisionCategories.Selected;
1178 prm.m_collisionFlags = 0;
1179 }
1180 else if (prm.m_isVolumeDetect)
1181 {
1182 prm.m_collisionCategories = CollisionCategories.VolumeDtc;
1183 if (m_isphysical)
1184 prm.m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character;
1185 else
1186 prm.m_collisionFlags = 0;
1187 }
1188 else if (prm.m_isphantom)
1189 {
1190 prm.m_collisionCategories = CollisionCategories.Phantom;
1191 if (m_isphysical)
1192 prm.m_collisionFlags = CollisionCategories.Land;
1193 else
1194 prm.m_collisionFlags = 0;
1195 }
1196 else
1197 {
1198 prm.m_collisionCategories = m_collisionCategories;
1199 prm.m_collisionFlags = m_collisionFlags;
1200 }
1201 }
1202
1203 if (prm.prim_geom != IntPtr.Zero)
1204 { 981 {
1205 if (prm.m_NoColide) 982 d.JointDestroy(Amotor);
1206 { 983 Amotor = IntPtr.Zero;
1207 d.GeomSetCategoryBits(prm.prim_geom, 0);
1208 if (m_isphysical)
1209 d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land);
1210 else
1211 d.GeomSetCollideBits(prm.prim_geom, 0);
1212 }
1213 else
1214 {
1215 d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories);
1216 d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags);
1217 }
1218 } 984 }
1219 } 985 }
1220 } 986 }
987 }
1221 988
1222 if (m_NoColide) 989 // Store this for later in case we get turned into a separate body
990 m_angularlock = m_taintAngularLock;
991 }
992
993 /// <summary>
994 /// Change prim in response to a link taint.
995 /// </summary>
996 private void changelink()
997 {
998 // If the newly set parent is not null
999 // create link
1000 if (_parent == null && m_taintparent != null)
1001 {
1002 if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim)
1223 { 1003 {
1224 d.GeomSetCategoryBits(prim_geom, 0); 1004 OdePrim obj = (OdePrim)m_taintparent;
1225 d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land); 1005 //obj.disableBody();
1226 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) 1006//Console.WriteLine("changelink calls ParentPrim");
1007 obj.AddChildPrim(this);
1008
1009 /*
1010 if (obj.Body != (IntPtr)0 && Body != (IntPtr)0 && obj.Body != Body)
1227 { 1011 {
1228 d.GeomSetCategoryBits(collide_geom, 0); 1012 _linkJointGroup = d.JointGroupCreate(0);
1229 d.GeomSetCollideBits(collide_geom, (uint)CollisionCategories.Land); 1013 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
1014 d.JointAttach(m_linkJoint, obj.Body, Body);
1015 d.JointSetFixed(m_linkJoint);
1230 } 1016 }
1017 */
1231 } 1018 }
1232 else 1019 }
1020 // If the newly set parent is null
1021 // destroy link
1022 else if (_parent != null && m_taintparent == null)
1023 {
1024//Console.WriteLine(" changelink B");
1025
1026 if (_parent is OdePrim)
1233 { 1027 {
1234 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); 1028 OdePrim obj = (OdePrim)_parent;
1235 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); 1029 obj.ChildDelink(this);
1236 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) 1030 childPrim = false;
1237 { 1031 //_parent = null;
1238 d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories);
1239 d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags);
1240 }
1241 } 1032 }
1033
1034 /*
1035 if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0)
1036 d.JointGroupDestroy(_linkJointGroup);
1037
1038 _linkJointGroup = (IntPtr)0;
1039 m_linkJoint = (IntPtr)0;
1040 */
1242 } 1041 }
1042
1043 _parent = m_taintparent;
1044 m_taintPhysics = IsPhysical;
1243 } 1045 }
1244 1046
1245 private void createAMotor(Vector3 axis) 1047 /// <summary>
1048 /// Add a child prim to this parent prim.
1049 /// </summary>
1050 /// <param name="prim">Child prim</param>
1051 private void AddChildPrim(OdePrim prim)
1246 { 1052 {
1247 if (Body == IntPtr.Zero) 1053 if (LocalID == prim.LocalID)
1248 return; 1054 return;
1249 1055
1250 if (Amotor != IntPtr.Zero) 1056 if (Body == IntPtr.Zero)
1251 { 1057 {
1252 d.JointDestroy(Amotor); 1058 Body = d.BodyCreate(_parent_scene.world);
1253 Amotor = IntPtr.Zero; 1059 setMass();
1254 } 1060 }
1255 1061
1256 int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z); 1062 lock (childrenPrim)
1257 1063 {
1258 if (axisnum <= 0) 1064 if (childrenPrim.Contains(prim))
1259 return; 1065 return;
1260
1261 // stop it
1262 d.BodySetTorque(Body, 0, 0, 0);
1263 d.BodySetAngularVel(Body, 0, 0, 0);
1264
1265 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
1266 d.JointAttach(Amotor, Body, IntPtr.Zero);
1267
1268 d.JointSetAMotorMode(Amotor, 0);
1269 1066
1270 d.JointSetAMotorNumAxes(Amotor, axisnum); 1067// m_log.DebugFormat(
1068// "[ODE PRIM]: Linking prim {0} {1} to {2} {3}", prim.Name, prim.LocalID, Name, LocalID);
1271 1069
1272 // get current orientation to lock 1070 childrenPrim.Add(prim);
1273 1071
1274 d.Quaternion dcur = d.BodyGetQuaternion(Body); 1072 foreach (OdePrim prm in childrenPrim)
1275 Quaternion curr; // crap convertion between identical things 1073 {
1276 curr.X = dcur.X; 1074 d.Mass m2;
1277 curr.Y = dcur.Y; 1075 d.MassSetZero(out m2);
1278 curr.Z = dcur.Z; 1076 d.MassSetBoxTotal(out m2, prim.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z);
1279 curr.W = dcur.W;
1280 Vector3 ax;
1281 1077
1282 int i = 0; 1078 d.Quaternion quat = new d.Quaternion();
1283 int j = 0; 1079 quat.W = prm._orientation.W;
1284 if (axis.X == 0) 1080 quat.X = prm._orientation.X;
1285 { 1081 quat.Y = prm._orientation.Y;
1286 ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X 1082 quat.Z = prm._orientation.Z;
1287 // ODE should do this with axis relative to body 1 but seems to fail
1288 d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z);
1289 d.JointSetAMotorAngle(Amotor, 0, 0);
1290 d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, -0.000001f);
1291 d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0.000001f);
1292 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
1293 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f);
1294 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f);
1295 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f);
1296 d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f);
1297 d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f);
1298 i++;
1299 j = 256; // move to next axis set
1300 }
1301 1083
1302 if (axis.Y == 0) 1084 d.Matrix3 mat = new d.Matrix3();
1303 { 1085 d.RfromQ(out mat, ref quat);
1304 ax = (new Vector3(0, 1, 0)) * curr; 1086 d.MassRotate(ref m2, ref mat);
1305 d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); 1087 d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z);
1306 d.JointSetAMotorAngle(Amotor, i, 0); 1088 d.MassAdd(ref pMass, ref m2);
1307 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); 1089 }
1308 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f);
1309 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
1310 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
1311 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
1312 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
1313 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
1314 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
1315 i++;
1316 j += 256;
1317 }
1318 1090
1319 if (axis.Z == 0) 1091 foreach (OdePrim prm in childrenPrim)
1320 { 1092 {
1321 ax = (new Vector3(0, 0, 1)) * curr; 1093 prm.m_collisionCategories |= CollisionCategories.Body;
1322 d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); 1094 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1323 d.JointSetAMotorAngle(Amotor, i, 0);
1324 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f);
1325 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f);
1326 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
1327 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
1328 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
1329 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
1330 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
1331 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
1332 }
1333 }
1334 1095
1096//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + Name);
1097 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
1098 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
1335 1099
1336 private void SetGeom(IntPtr geom) 1100 d.Quaternion quat = new d.Quaternion();
1337 { 1101 quat.W = prm._orientation.W;
1338 prim_geom = geom; 1102 quat.X = prm._orientation.X;
1339 //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); 1103 quat.Y = prm._orientation.Y;
1340 if (prim_geom != IntPtr.Zero) 1104 quat.Z = prm._orientation.Z;
1341 {
1342 1105
1343 if (m_NoColide) 1106 d.Matrix3 mat = new d.Matrix3();
1344 { 1107 d.RfromQ(out mat, ref quat);
1345 d.GeomSetCategoryBits(prim_geom, 0); 1108 if (Body != IntPtr.Zero)
1346 if (m_isphysical)
1347 { 1109 {
1348 d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land); 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);
1349 } 1121 }
1350 else 1122 else
1351 { 1123 {
1352 d.GeomSetCollideBits(prim_geom, 0); 1124 m_log.DebugFormat("[PHYSICS]: {0} ain't got no boooooooooddy, no body", Name);
1353 d.GeomDisable(prim_geom);
1354 } 1125 }
1355 } 1126
1356 else 1127 prm.m_interpenetrationcount = 0;
1128 prm.m_collisionscore = 0;
1129 prm.m_disabled = false;
1130
1131 // The body doesn't already have a finite rotation mode set here
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
1173 // The body doesn't already have a finite rotation mode set here
1174 if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
1357 { 1175 {
1358 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); 1176 createAMotor(m_angularlock);
1359 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
1360 } 1177 }
1361 1178
1362 UpdatePrimBodyData(); 1179 d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
1363 _parent_scene.actor_name_map[prim_geom] = this;
1364 1180
1181 if (m_vehicle.Type != Vehicle.TYPE_NONE)
1182 m_vehicle.Enable(Body, _parent_scene);
1365 1183
1366// debug 1184 _parent_scene.ActivatePrim(this);
1367 d.AABB aabb; 1185 }
1368 d.GeomGetAABB(prim_geom, out aabb); 1186 }
1369 float x = aabb.MaxX - aabb.MinX;
1370 float y = aabb.MaxY - aabb.MinY;
1371 float z = aabb.MaxZ - aabb.MinZ;
1372 if( x > 60.0f || y > 60.0f || z > 60.0f)
1373 m_log.WarnFormat("[PHYSICS]: large prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}",
1374 Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString());
1375 else if (x < 0.001f || y < 0.001f || z < 0.001f)
1376 m_log.WarnFormat("[PHYSICS]: small prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}",
1377 Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString());
1378 1187
1379// 1188 private void ChildSetGeom(OdePrim odePrim)
1189 {
1190// m_log.DebugFormat(
1191// "[ODE PRIM]: ChildSetGeom {0} {1} for {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID);
1380 1192
1193 //if (IsPhysical && Body != IntPtr.Zero)
1194 lock (childrenPrim)
1195 {
1196 foreach (OdePrim prm in childrenPrim)
1197 {
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 }
1381 1207
1208 disableBody();
1382 1209
1210 // Spurious - Body == IntPtr.Zero after disableBody()
1211// if (Body != IntPtr.Zero)
1212// {
1213// _parent_scene.DeactivatePrim(this);
1214// }
1383 1215
1216 lock (childrenPrim)
1217 {
1218 foreach (OdePrim prm in childrenPrim)
1219 {
1220//Console.WriteLine("ChildSetGeom calls ParentPrim");
1221 AddChildPrim(prm);
1222 }
1384 } 1223 }
1385 else
1386 m_log.Warn("Setting bad Geom");
1387 } 1224 }
1388 1225
1389 private bool GetMeshGeom() 1226 private void ChildDelink(OdePrim odePrim)
1390 { 1227 {
1391 IntPtr vertices, indices; 1228// m_log.DebugFormat(
1392 int vertexCount, indexCount; 1229// "[ODE PRIM]: Delinking prim {0} {1} from {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID);
1393 int vertexStride, triStride;
1394
1395 IMesh mesh = m_mesh;
1396 1230
1397 if (mesh == null) 1231 // Okay, we have a delinked child.. need to rebuild the body.
1398 return false; 1232 lock (childrenPrim)
1233 {
1234 foreach (OdePrim prm in childrenPrim)
1235 {
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 }
1399 1245
1400 mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); 1246 disableBody();
1401 mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount);
1402 1247
1403 if (vertexCount == 0 || indexCount == 0) 1248 lock (childrenPrim)
1404 { 1249 {
1405 m_log.WarnFormat("[PHYSICS]: Invalid mesh data on OdePrim {0}, mesh {1}", 1250 //Console.WriteLine("childrenPrim.Remove " + odePrim);
1406 Name, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh"); 1251 childrenPrim.Remove(odePrim);
1407 1252 }
1408 m_hasOBB = false;
1409 m_OBBOffset = Vector3.Zero;
1410 m_OBB = _size * 0.5f;
1411 1253
1412 m_physCost = 0.1f; 1254 // Spurious - Body == IntPtr.Zero after disableBody()
1413 m_streamCost = 1.0f; 1255// if (Body != IntPtr.Zero)
1256// {
1257// _parent_scene.DeactivatePrim(this);
1258// }
1414 1259
1415 _parent_scene.mesher.ReleaseMesh(mesh); 1260 lock (childrenPrim)
1416 m_meshState = MeshState.MeshFailed; 1261 {
1417 m_mesh = null; 1262 foreach (OdePrim prm in childrenPrim)
1418 return false; 1263 {
1264//Console.WriteLine("ChildDelink calls ParentPrim");
1265 AddChildPrim(prm);
1266 }
1419 } 1267 }
1268 }
1420 1269
1421 IntPtr geo = IntPtr.Zero; 1270 /// <summary>
1422 1271 /// Change prim in response to a selection taint.
1423 try 1272 /// </summary>
1273 private void changeSelectedStatus()
1274 {
1275 if (m_taintselected)
1424 { 1276 {
1425 _triMeshData = d.GeomTriMeshDataCreate(); 1277 m_collisionCategories = CollisionCategories.Selected;
1278 m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
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 {
1304 disableBodySoft();
1305 }
1426 1306
1427 d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); 1307 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1428 d.GeomTriMeshDataPreprocess(_triMeshData); 1308 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1429 1309
1430 geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null); 1310 if (IsPhysical)
1311 {
1312 disableBodySoft();
1313 }
1431 } 1314 }
1432 1315 else
1433 catch (Exception e)
1434 { 1316 {
1435 m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e); 1317 m_collisionCategories = CollisionCategories.Geom;
1436 if (_triMeshData != IntPtr.Zero) 1318
1319 if (IsPhysical)
1320 m_collisionCategories |= CollisionCategories.Body;
1321
1322 m_collisionFlags = m_default_collisionFlags;
1323
1324 if (m_collidesLand)
1325 m_collisionFlags |= CollisionCategories.Land;
1326 if (m_collidesWater)
1327 m_collisionFlags |= CollisionCategories.Water;
1328
1329 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1330 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1331
1332 if (IsPhysical)
1437 { 1333 {
1438 try 1334 if (Body != IntPtr.Zero)
1439 {
1440 d.GeomTriMeshDataDestroy(_triMeshData);
1441 }
1442 catch
1443 { 1335 {
1336 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1337 d.BodySetForce(Body, 0, 0, 0);
1338 enableBodySoft();
1444 } 1339 }
1445 } 1340 }
1446 _triMeshData = IntPtr.Zero;
1447
1448 m_hasOBB = false;
1449 m_OBBOffset = Vector3.Zero;
1450 m_OBB = _size * 0.5f;
1451 m_physCost = 0.1f;
1452 m_streamCost = 1.0f;
1453
1454 _parent_scene.mesher.ReleaseMesh(mesh);
1455 m_meshState = MeshState.MeshFailed;
1456 m_mesh = null;
1457 return false;
1458 } 1341 }
1459 1342
1460 m_physCost = 0.0013f * (float)indexCount; 1343 resetCollisionAccounting();
1461 // todo 1344 m_isSelected = m_taintselected;
1462 m_streamCost = 1.0f; 1345 }//end changeSelectedStatus
1463
1464 SetGeom(geo);
1465 1346
1466 return true; 1347 internal void ResetTaints()
1348 {
1349 m_taintposition = _position;
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;
1467 } 1358 }
1468 1359
1469 private void CreateGeom() 1360 /// <summary>
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)
1470 { 1366 {
1471 bool hasMesh = false; 1367#if SPAM
1472 1368Console.WriteLine("CreateGeom:");
1473 m_NoColide = false; 1369#endif
1474 1370 if (mesh != null)
1475 if ((m_meshState & MeshState.FailMask) != 0)
1476 m_NoColide = true;
1477
1478 else if(m_mesh != null)
1479 { 1371 {
1480 if (GetMeshGeom()) 1372 setMesh(_parent_scene, mesh);
1481 hasMesh = true;
1482 else
1483 m_NoColide = true;
1484 } 1373 }
1485 1374 else
1486
1487 if (!hasMesh)
1488 { 1375 {
1489 IntPtr geo = IntPtr.Zero; 1376 if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1)
1490 1377 {
1491 if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1 1378 if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z)
1492 && _size.X == _size.Y && _size.Y == _size.Z)
1493 { // it's a sphere
1494 try
1495 { 1379 {
1496 geo = d.CreateSphere(m_targetSpace, _size.X * 0.5f); 1380 if (((_size.X / 2f) > 0f))
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 }
1497 } 1410 }
1498 catch (Exception e) 1411 else
1499 { 1412 {
1500 m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e); 1413// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1501 return; 1414 try
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 }
1502 } 1425 }
1503 } 1426 }
1504 else 1427 else
1505 {// do it as a box 1428 {
1429// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1506 try 1430 try
1507 { 1431 {
1508 geo = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z); 1432//Console.WriteLine(" CreateGeom 4");
1433 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1434 m_expectedCollisionContacts = 4;
1509 } 1435 }
1510 catch (Exception e) 1436 catch (AccessViolationException)
1511 { 1437 {
1512 m_log.Warn("[PHYSICS]: Create box failed: {0}", e); 1438 m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
1513 return; 1439 return;
1514 } 1440 }
1515 } 1441 }
1516 m_physCost = 0.1f;
1517 m_streamCost = 1.0f;
1518 SetGeom(geo);
1519 } 1442 }
1520 } 1443 }
1521 1444
1522 private void RemoveGeom() 1445 /// <summary>
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()
1523 { 1452 {
1524 if (prim_geom != IntPtr.Zero) 1453 if (prim_geom != IntPtr.Zero)
1525 { 1454 {
1526 _parent_scene.actor_name_map.Remove(prim_geom);
1527
1528 try 1455 try
1529 { 1456 {
1457 _parent_scene.geom_name_map.Remove(prim_geom);
1458 _parent_scene.actor_name_map.Remove(prim_geom);
1530 d.GeomDestroy(prim_geom); 1459 d.GeomDestroy(prim_geom);
1531 if (_triMeshData != IntPtr.Zero) 1460 m_expectedCollisionContacts = 0;
1532 { 1461 prim_geom = IntPtr.Zero;
1533 d.GeomTriMeshDataDestroy(_triMeshData);
1534 _triMeshData = IntPtr.Zero;
1535 }
1536 } 1462 }
1537 catch (Exception e) 1463 catch (System.AccessViolationException)
1538 { 1464 {
1539 m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e); 1465 prim_geom = IntPtr.Zero;
1466 m_expectedCollisionContacts = 0;
1467 m_log.ErrorFormat("[PHYSICS]: PrimGeom dead for {0}", Name);
1468
1469 return false;
1540 } 1470 }
1541 1471
1542 prim_geom = IntPtr.Zero; 1472 return true;
1543 collide_geom = IntPtr.Zero;
1544 m_targetSpace = IntPtr.Zero;
1545 } 1473 }
1546 else 1474 else
1547 { 1475 {
1548 m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name); 1476 m_log.WarnFormat(
1549 } 1477 "[ODE PRIM]: Called RemoveGeom() on {0} {1} where geometry was already null.", Name, LocalID);
1550
1551 lock (m_meshlock)
1552 {
1553 if (m_mesh != null)
1554 {
1555 _parent_scene.mesher.ReleaseMesh(m_mesh);
1556 m_mesh = null;
1557 }
1558 }
1559
1560 Body = IntPtr.Zero;
1561 m_hasOBB = false;
1562 }
1563
1564 //sets non physical prim m_targetSpace to right space in spaces grid for static prims
1565 // should only be called for non physical prims unless they are becoming non physical
1566 private void SetInStaticSpace(OdePrim prim)
1567 {
1568 IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace);
1569 prim.m_targetSpace = targetSpace;
1570 collide_geom = IntPtr.Zero;
1571 }
1572
1573 public void enableBodySoft()
1574 {
1575 m_disabled = false;
1576 if (!childPrim && !m_isSelected)
1577 {
1578 if (m_isphysical && Body != IntPtr.Zero)
1579 {
1580 UpdateCollisionCatFlags();
1581 ApplyCollisionCatFlags();
1582
1583 d.BodyEnable(Body);
1584 }
1585 }
1586 resetCollisionAccounting();
1587 }
1588 1478
1589 private void disableBodySoft() 1479 return false;
1590 {
1591 m_disabled = true;
1592 if (!childPrim)
1593 {
1594 if (m_isphysical && Body != IntPtr.Zero)
1595 {
1596 if (m_isSelected)
1597 m_collisionFlags = CollisionCategories.Selected;
1598 else
1599 m_collisionCategories = 0;
1600 m_collisionFlags = 0;
1601 ApplyCollisionCatFlags();
1602 d.BodyDisable(Body);
1603 }
1604 } 1480 }
1605 } 1481 }
1606 1482 /// <summary>
1607 private void MakeBody() 1483 /// Add prim in response to an add taint.
1484 /// </summary>
1485 private void changeadd()
1608 { 1486 {
1609 if (!m_isphysical) // only physical get bodies 1487// m_log.DebugFormat("[ODE PRIM]: Adding prim {0}", Name);
1610 return; 1488
1489 int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
1490 IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
1611 1491
1612 if (childPrim) // child prims don't get bodies; 1492 if (targetspace == IntPtr.Zero)
1613 return; 1493 targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
1614 1494
1615 if (m_building) 1495 m_targetSpace = targetspace;
1616 return;
1617 1496
1618 if (prim_geom == IntPtr.Zero) 1497 IMesh mesh = null;
1619 {
1620 m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet");
1621 return;
1622 }
1623 1498
1624 if (Body != IntPtr.Zero) 1499 if (_parent_scene.needsMeshing(_pbs))
1625 { 1500 {
1626 DestroyBody(); 1501 // Don't need to re-enable body.. it's done in SetMesh
1627 m_log.Warn("[PHYSICS]: MakeBody called having a body"); 1502 mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
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();
1628 } 1507 }
1629 1508
1630 if (d.GeomGetBody(prim_geom) != IntPtr.Zero) 1509#if SPAM
1631 { 1510Console.WriteLine("changeadd 1");
1632 d.GeomSetBody(prim_geom, IntPtr.Zero); 1511#endif
1633 m_log.Warn("[PHYSICS]: MakeBody root geom already had a body"); 1512 CreateGeom(m_targetSpace, mesh);
1634 }
1635 1513
1636 d.Matrix3 mymat = new d.Matrix3(); 1514 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
1637 d.Quaternion myrot = new d.Quaternion(); 1515 d.Quaternion myrot = new d.Quaternion();
1638 d.Mass objdmass = new d.Mass { };
1639
1640 Body = d.BodyCreate(_parent_scene.world);
1641
1642 objdmass = primdMass;
1643
1644 // rotate inertia
1645 myrot.X = _orientation.X; 1516 myrot.X = _orientation.X;
1646 myrot.Y = _orientation.Y; 1517 myrot.Y = _orientation.Y;
1647 myrot.Z = _orientation.Z; 1518 myrot.Z = _orientation.Z;
1648 myrot.W = _orientation.W; 1519 myrot.W = _orientation.W;
1520 d.GeomSetQuaternion(prim_geom, ref myrot);
1649 1521
1650 d.RfromQ(out mymat, ref myrot); 1522 if (IsPhysical && Body == IntPtr.Zero)
1651 d.MassRotate(ref objdmass, ref mymat); 1523 enableBody();
1652
1653 // set the body rotation
1654 d.BodySetRotation(Body, ref mymat);
1655 1524
1656 // recompute full object inertia if needed 1525 changeSelectedStatus();
1657 if (childrenPrim.Count > 0)
1658 {
1659 d.Matrix3 mat = new d.Matrix3();
1660 d.Quaternion quat = new d.Quaternion();
1661 d.Mass tmpdmass = new d.Mass { };
1662 Vector3 rcm;
1663 1526
1664 rcm.X = _position.X; 1527 m_taintadd = false;
1665 rcm.Y = _position.Y; 1528 }
1666 rcm.Z = _position.Z;
1667 1529
1668 lock (childrenPrim) 1530 /// <summary>
1531 /// Move prim in response to a move taint.
1532 /// </summary>
1533 private void changemove()
1534 {
1535 if (IsPhysical)
1536 {
1537 if (!m_disabled && !m_taintremove && !childPrim)
1669 { 1538 {
1670 foreach (OdePrim prm in childrenPrim) 1539 if (Body == IntPtr.Zero)
1540 enableBody();
1541
1542 //Prim auto disable after 20 frames,
1543 //if you move it, re-enable the prim manually.
1544 if (_parent != null)
1671 { 1545 {
1672 if (prm.prim_geom == IntPtr.Zero) 1546 if (m_linkJoint != IntPtr.Zero)
1673 { 1547 {
1674 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet"); 1548 d.JointDestroy(m_linkJoint);
1675 continue; 1549 m_linkJoint = IntPtr.Zero;
1676 } 1550 }
1551 }
1552
1553 if (Body != IntPtr.Zero)
1554 {
1555 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
1677 1556
1678 tmpdmass = prm.primdMass; 1557 if (_parent != null)
1679
1680 // apply prim current rotation to inertia
1681 quat.X = prm._orientation.X;
1682 quat.Y = prm._orientation.Y;
1683 quat.Z = prm._orientation.Z;
1684 quat.W = prm._orientation.W;
1685 d.RfromQ(out mat, ref quat);
1686 d.MassRotate(ref tmpdmass, ref mat);
1687
1688 Vector3 ppos = prm._position;
1689 ppos.X -= rcm.X;
1690 ppos.Y -= rcm.Y;
1691 ppos.Z -= rcm.Z;
1692 // refer inertia to root prim center of mass position
1693 d.MassTranslate(ref tmpdmass,
1694 ppos.X,
1695 ppos.Y,
1696 ppos.Z);
1697
1698 d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia
1699 // fix prim colision cats
1700
1701 if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero)
1702 { 1558 {
1703 d.GeomSetBody(prm.prim_geom, IntPtr.Zero); 1559 OdePrim odParent = (OdePrim)_parent;
1704 m_log.Warn("[PHYSICS]: MakeBody child geom already had a body"); 1560 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
1561 {
1562// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
1563Console.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 }
1705 } 1568 }
1706 1569 d.BodyEnable(Body);
1707 d.GeomClearOffset(prm.prim_geom); 1570 if (m_vehicle.Type != Vehicle.TYPE_NONE)
1708 d.GeomSetBody(prm.prim_geom, Body); 1571 {
1709 prm.Body = Body; 1572 m_vehicle.Enable(Body, _parent_scene);
1710 d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation 1573 }
1574 }
1575 else
1576 {
1577 m_log.WarnFormat("[PHYSICS]: Body for {0} still null after enableBody(). This is a crash scenario.", Name);
1711 } 1578 }
1712 } 1579 }
1580 //else
1581 // {
1582 //m_log.Debug("[BUG]: race!");
1583 //}
1713 } 1584 }
1714 1585
1715 d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset 1586 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
1716 // associate root geom with body 1587 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
1717 d.GeomSetBody(prim_geom, Body); 1588// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1718
1719 d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z);
1720 d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z);
1721
1722 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
1723 myrot.X = -myrot.X;
1724 myrot.Y = -myrot.Y;
1725 myrot.Z = -myrot.Z;
1726 1589
1727 d.RfromQ(out mymat, ref myrot); 1590 IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace);
1728 d.MassRotate(ref objdmass, ref mymat); 1591 m_targetSpace = tempspace;
1729 1592
1730 d.BodySetMass(Body, ref objdmass); 1593// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1731 _mass = objdmass.mass;
1732 1594
1733 // disconnect from world gravity so we can apply buoyancy 1595 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
1734 d.BodySetGravityMode(Body, false);
1735
1736 d.BodySetAutoDisableFlag(Body, true);
1737 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
1738 d.BodySetDamping(Body, .005f, .005f);
1739
1740 if (m_targetSpace != IntPtr.Zero)
1741 {
1742 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1743 if (d.SpaceQuery(m_targetSpace, prim_geom))
1744 d.SpaceRemove(m_targetSpace, prim_geom);
1745 }
1746
1747 if (childrenPrim.Count == 0)
1748 {
1749 collide_geom = prim_geom;
1750 m_targetSpace = _parent_scene.ActiveSpace;
1751 }
1752 else
1753 {
1754 m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace);
1755 d.HashSpaceSetLevels(m_targetSpace, -2, 8);
1756 d.SpaceSetSublevel(m_targetSpace, 3);
1757 d.SpaceSetCleanup(m_targetSpace, false);
1758
1759 d.GeomSetCategoryBits(m_targetSpace, (uint)(CollisionCategories.Space |
1760 CollisionCategories.Geom |
1761 CollisionCategories.Phantom |
1762 CollisionCategories.VolumeDtc
1763 ));
1764 d.GeomSetCollideBits(m_targetSpace, 0);
1765 collide_geom = m_targetSpace;
1766 }
1767 1596
1597// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1768 d.SpaceAdd(m_targetSpace, prim_geom); 1598 d.SpaceAdd(m_targetSpace, prim_geom);
1769 1599
1770 if (m_delaySelect) 1600 changeSelectedStatus();
1771 {
1772 m_isSelected = true;
1773 m_delaySelect = false;
1774 }
1775
1776 m_collisionscore = 0;
1777 1601
1778 UpdateCollisionCatFlags(); 1602 resetCollisionAccounting();
1779 ApplyCollisionCatFlags(); 1603 m_taintposition = _position;
1604 }
1780 1605
1781 _parent_scene.addActivePrim(this); 1606 internal void Move(float timestep)
1607 {
1608 float fx = 0;
1609 float fy = 0;
1610 float fz = 0;
1782 1611
1783 lock (childrenPrim) 1612 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
1784 { 1613 {
1785 foreach (OdePrim prm in childrenPrim) 1614 if (m_vehicle.Type != Vehicle.TYPE_NONE)
1786 { 1615 {
1787 if (prm.prim_geom == IntPtr.Zero) 1616 // 'VEHICLES' are dealt with in ODEDynamics.cs
1788 continue; 1617 m_vehicle.Step(timestep, _parent_scene);
1618 }
1619 else
1620 {
1621//Console.WriteLine("Move " + Name);
1622 if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
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());
1789 1643
1790 Vector3 ppos = prm._position; 1644
1791 d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position 1645 //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
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;
1792 1650
1793 if (prm.m_targetSpace != m_targetSpace) 1651 if (m_usePID)
1794 { 1652 {
1795 if (prm.m_targetSpace != IntPtr.Zero) 1653//Console.WriteLine("PID " + Name);
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))
1796 { 1667 {
1797 _parent_scene.waitForSpaceUnlock(prm.m_targetSpace); 1668 //PID_G = PID_G / m_PIDTau;
1798 if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom)) 1669 m_PIDTau = 1;
1799 d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);
1800 } 1670 }
1801 prm.m_targetSpace = m_targetSpace; 1671
1802 d.SpaceAdd(m_targetSpace, prm.prim_geom); 1672 if ((PID_G - m_PIDTau) <= 0)
1803 } 1673 {
1804 1674 PID_G = m_PIDTau + 1;
1805 prm.m_collisionscore = 0; 1675 }
1676 //PidStatus = true;
1806 1677
1807 if(!m_disabled) 1678 // PhysicsVector vec = new PhysicsVector();
1808 prm.m_disabled = false; 1679 d.Vector3 vel = d.BodyGetLinearVel(Body);
1809 1680
1810 _parent_scene.addActivePrim(prm); 1681 d.Vector3 pos = d.BodyGetPosition(Body);
1811 } 1682 _target_velocity =
1812 } 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 );
1813 1688
1814 // The body doesn't already have a finite rotation mode set here 1689 // if velocity is zero, use position control; otherwise, velocity control
1815 if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null)
1816 {
1817 createAMotor(m_angularlock);
1818 }
1819 1690
1691 if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
1692 {
1693 // keep track of where we stopped. No more slippin' & slidin'
1694
1695 // We only want to deactivate the PID Controller if we think we want to have our surrogate
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;
1820 1711
1821 if (m_isSelected || m_disabled) 1712 // We're flying and colliding with something
1822 { 1713 fx = ((_target_velocity.X) - vel.X) * (PID_D);
1823 d.BodyDisable(Body); 1714 fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
1824 } 1715
1825 else 1716 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
1826 {
1827 d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z);
1828 d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
1829 }
1830 _parent_scene.addActiveGroups(this);
1831 }
1832 1717
1833 private void DestroyBody() 1718 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1834 { 1719 }
1835 if (Body != IntPtr.Zero) 1720 } // end if (m_usePID)
1836 {
1837 _parent_scene.remActivePrim(this);
1838
1839 collide_geom = IntPtr.Zero;
1840 1721
1841 if (m_disabled) 1722 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
1842 m_collisionCategories = 0; 1723 if (m_useHoverPID && !m_usePID)
1843 else if (m_isSelected) 1724 {
1844 m_collisionCategories = CollisionCategories.Selected; 1725//Console.WriteLine("Hover " + Name);
1845 else if (m_isVolumeDetect) 1726
1846 m_collisionCategories = CollisionCategories.VolumeDtc; 1727 // If we're using the PID controller, then we have no gravity
1847 else if (m_isphantom) 1728 fz = (-1 * _parent_scene.gravityz) * m_mass;
1848 m_collisionCategories = CollisionCategories.Phantom;
1849 else
1850 m_collisionCategories = CollisionCategories.Geom;
1851 1729
1852 m_collisionFlags = 0; 1730 // no lock; for now it's only called from within Simulate()
1853 1731
1854 if (prim_geom != IntPtr.Zero) 1732 // If the PID Controller isn't active then we set our force
1855 { 1733 // calculating base velocity to the current position
1856 if (m_NoColide)
1857 {
1858 d.GeomSetCategoryBits(prim_geom, 0);
1859 d.GeomSetCollideBits(prim_geom, 0);
1860 }
1861 else
1862 {
1863 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
1864 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
1865 }
1866 UpdateDataFromGeom();
1867 d.GeomSetBody(prim_geom, IntPtr.Zero);
1868 SetInStaticSpace(this);
1869 }
1870 1734
1871 if (!childPrim) 1735 if ((m_PIDTau < 1))
1872 {
1873 lock (childrenPrim)
1874 {
1875 foreach (OdePrim prm in childrenPrim)
1876 { 1736 {
1877 _parent_scene.remActivePrim(prm); 1737 PID_G = PID_G / m_PIDTau;
1738 }
1878 1739
1879 if (prm.m_isSelected) 1740 if ((PID_G - m_PIDTau) <= 0)
1880 prm.m_collisionCategories = CollisionCategories.Selected; 1741 {
1881 else if (prm.m_isVolumeDetect) 1742 PID_G = m_PIDTau + 1;
1882 prm.m_collisionCategories = CollisionCategories.VolumeDtc; 1743 }
1883 else if (prm.m_isphantom)
1884 prm.m_collisionCategories = CollisionCategories.Phantom;
1885 else
1886 prm.m_collisionCategories = CollisionCategories.Geom;
1887 1744
1888 prm.m_collisionFlags = 0; 1745 // Where are we, and where are we headed?
1746 d.Vector3 pos = d.BodyGetPosition(Body);
1747 d.Vector3 vel = d.BodyGetLinearVel(Body);
1889 1748
1890 if (prm.prim_geom != IntPtr.Zero) 1749 // Non-Vehicles have a limited set of Hover options.
1891 { 1750 // determine what our target height really is based on HoverType
1892 if (prm.m_NoColide) 1751 switch (m_PIDHoverType)
1752 {
1753 case PIDHoverType.Ground:
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)
1893 { 1761 {
1894 d.GeomSetCategoryBits(prm.prim_geom, 0); 1762 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1895 d.GeomSetCollideBits(prm.prim_geom, 0);
1896 } 1763 }
1897 else 1764 else
1898 { 1765 {
1899 d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories); 1766 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
1900 d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags);
1901 } 1767 }
1902 prm.UpdateDataFromGeom(); 1768 break;
1903 SetInStaticSpace(prm);
1904 }
1905 prm.Body = IntPtr.Zero;
1906 prm._mass = prm.primMass;
1907 prm.m_collisionscore = 0;
1908 }
1909 }
1910 if (Amotor != IntPtr.Zero)
1911 {
1912 d.JointDestroy(Amotor);
1913 Amotor = IntPtr.Zero;
1914 }
1915 _parent_scene.remActiveGroup(this);
1916 d.BodyDestroy(Body);
1917 }
1918 Body = IntPtr.Zero;
1919 }
1920 _mass = primMass;
1921 m_collisionscore = 0;
1922 }
1923
1924 private void FixInertia(Vector3 NewPos,Quaternion newrot)
1925 {
1926 d.Matrix3 mat = new d.Matrix3();
1927 d.Quaternion quat = new d.Quaternion();
1928
1929 d.Mass tmpdmass = new d.Mass { };
1930 d.Mass objdmass = new d.Mass { };
1931
1932 d.BodyGetMass(Body, out tmpdmass);
1933 objdmass = tmpdmass;
1934
1935 d.Vector3 dobjpos;
1936 d.Vector3 thispos;
1937 1769
1938 // get current object position and rotation 1770 } // end switch (m_PIDHoverType)
1939 dobjpos = d.BodyGetPosition(Body);
1940 1771
1941 // get prim own inertia in its local frame
1942 tmpdmass = primdMass;
1943 1772
1944 // transform to object frame 1773 _target_velocity =
1945 mat = d.GeomGetOffsetRotation(prim_geom); 1774 new Vector3(0.0f, 0.0f,
1946 d.MassRotate(ref tmpdmass, ref mat); 1775 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
1776 );
1947 1777
1948 thispos = d.GeomGetOffsetPosition(prim_geom); 1778 // if velocity is zero, use position control; otherwise, velocity control
1949 d.MassTranslate(ref tmpdmass,
1950 thispos.X,
1951 thispos.Y,
1952 thispos.Z);
1953 1779
1954 // subtract current prim inertia from object 1780 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
1955 DMassSubPartFromObj(ref tmpdmass, ref objdmass); 1781 {
1956 1782 // keep track of where we stopped. No more slippin' & slidin'
1957 // back prim own inertia 1783
1958 tmpdmass = primdMass; 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;
1959 1797
1960 // update to new position and orientation 1798 // We're flying and colliding with something
1961 _position = NewPos; 1799 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1962 d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z); 1800 }
1963 _orientation = newrot; 1801 }
1964 quat.X = newrot.X;
1965 quat.Y = newrot.Y;
1966 quat.Z = newrot.Z;
1967 quat.W = newrot.W;
1968 d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);
1969 1802
1970 mat = d.GeomGetOffsetRotation(prim_geom); 1803 fx *= m_mass;
1971 d.MassRotate(ref tmpdmass, ref mat); 1804 fy *= m_mass;
1805 //fz *= m_mass;
1972 1806
1973 thispos = d.GeomGetOffsetPosition(prim_geom); 1807 fx += m_force.X;
1974 d.MassTranslate(ref tmpdmass, 1808 fy += m_force.Y;
1975 thispos.X, 1809 fz += m_force.Z;
1976 thispos.Y,
1977 thispos.Z);
1978 1810
1979 d.MassAdd(ref objdmass, ref tmpdmass); 1811 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
1812 if (fx != 0 || fy != 0 || fz != 0)
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 }
1980 1826
1981 // fix all positions 1827 // 35x10 = 350n times the mass per second applied maximum.
1982 IntPtr g = d.BodyGetFirstGeom(Body); 1828 float nmax = 35f * m_mass;
1983 while (g != IntPtr.Zero) 1829 float nmin = -35f * m_mass;
1984 { 1830
1985 thispos = d.GeomGetOffsetPosition(g); 1831 if (fx > nmax)
1986 thispos.X -= objdmass.c.X; 1832 fx = nmax;
1987 thispos.Y -= objdmass.c.Y; 1833 if (fx < nmin)
1988 thispos.Z -= objdmass.c.Z; 1834 fx = nmin;
1989 d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); 1835 if (fy > nmax)
1990 g = d.dBodyGetNextGeom(g); 1836 fy = nmax;
1837 if (fy < nmin)
1838 fy = nmin;
1839 d.BodyAddForce(Body, fx, fy, fz);
1840//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
1841 }
1842 }
1843 }
1844 else
1845 { // is not physical, or is not a body or is selected
1846 // _zeroPosition = d.BodyGetPosition(Body);
1847 return;
1848//Console.WriteLine("Nothing " + Name);
1849
1991 } 1850 }
1992 d.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos);
1993
1994 d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
1995 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
1996 d.BodySetMass(Body, ref objdmass);
1997 _mass = objdmass.mass;
1998 } 1851 }
1999 1852
2000 1853 private void rotate()
2001
2002 private void FixInertia(Vector3 NewPos)
2003 { 1854 {
2004 d.Matrix3 primmat = new d.Matrix3(); 1855 d.Quaternion myrot = new d.Quaternion();
2005 d.Mass tmpdmass = new d.Mass { }; 1856 myrot.X = _orientation.X;
2006 d.Mass objdmass = new d.Mass { }; 1857 myrot.Y = _orientation.Y;
2007 d.Mass primmass = new d.Mass { }; 1858 myrot.Z = _orientation.Z;
2008 1859 myrot.W = _orientation.W;
2009 d.Vector3 dobjpos; 1860 if (Body != IntPtr.Zero)
2010 d.Vector3 thispos;
2011
2012 d.BodyGetMass(Body, out objdmass);
2013
2014 // get prim own inertia in its local frame
2015 primmass = primdMass;
2016 // transform to object frame
2017 primmat = d.GeomGetOffsetRotation(prim_geom);
2018 d.MassRotate(ref primmass, ref primmat);
2019
2020 tmpdmass = primmass;
2021
2022 thispos = d.GeomGetOffsetPosition(prim_geom);
2023 d.MassTranslate(ref tmpdmass,
2024 thispos.X,
2025 thispos.Y,
2026 thispos.Z);
2027
2028 // subtract current prim inertia from object
2029 DMassSubPartFromObj(ref tmpdmass, ref objdmass);
2030
2031 // update to new position
2032 _position = NewPos;
2033 d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z);
2034
2035 thispos = d.GeomGetOffsetPosition(prim_geom);
2036 d.MassTranslate(ref primmass,
2037 thispos.X,
2038 thispos.Y,
2039 thispos.Z);
2040
2041 d.MassAdd(ref objdmass, ref primmass);
2042
2043 // fix all positions
2044 IntPtr g = d.BodyGetFirstGeom(Body);
2045 while (g != IntPtr.Zero)
2046 { 1861 {
2047 thispos = d.GeomGetOffsetPosition(g); 1862 // KF: If this is a root prim do BodySet
2048 thispos.X -= objdmass.c.X; 1863 d.BodySetQuaternion(Body, ref myrot);
2049 thispos.Y -= objdmass.c.Y; 1864 if (IsPhysical)
2050 thispos.Z -= objdmass.c.Z; 1865 {
2051 d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); 1866 if (!m_angularlock.ApproxEquals(Vector3.One, 0f))
2052 g = d.dBodyGetNextGeom(g); 1867 createAMotor(m_angularlock);
1868 }
2053 } 1869 }
2054 1870 else
2055 d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos);
2056
2057 // get current object position and rotation
2058 dobjpos = d.BodyGetPosition(Body);
2059
2060 d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
2061 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
2062 d.BodySetMass(Body, ref objdmass);
2063 _mass = objdmass.mass;
2064 }
2065
2066 private void FixInertia(Quaternion newrot)
2067 {
2068 d.Matrix3 mat = new d.Matrix3();
2069 d.Quaternion quat = new d.Quaternion();
2070
2071 d.Mass tmpdmass = new d.Mass { };
2072 d.Mass objdmass = new d.Mass { };
2073 d.Vector3 dobjpos;
2074 d.Vector3 thispos;
2075
2076 d.BodyGetMass(Body, out objdmass);
2077
2078 // get prim own inertia in its local frame
2079 tmpdmass = primdMass;
2080 mat = d.GeomGetOffsetRotation(prim_geom);
2081 d.MassRotate(ref tmpdmass, ref mat);
2082 // transform to object frame
2083 thispos = d.GeomGetOffsetPosition(prim_geom);
2084 d.MassTranslate(ref tmpdmass,
2085 thispos.X,
2086 thispos.Y,
2087 thispos.Z);
2088
2089 // subtract current prim inertia from object
2090 DMassSubPartFromObj(ref tmpdmass, ref objdmass);
2091
2092 // update to new orientation
2093 _orientation = newrot;
2094 quat.X = newrot.X;
2095 quat.Y = newrot.Y;
2096 quat.Z = newrot.Z;
2097 quat.W = newrot.W;
2098 d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);
2099
2100 tmpdmass = primdMass;
2101 mat = d.GeomGetOffsetRotation(prim_geom);
2102 d.MassRotate(ref tmpdmass, ref mat);
2103 d.MassTranslate(ref tmpdmass,
2104 thispos.X,
2105 thispos.Y,
2106 thispos.Z);
2107
2108 d.MassAdd(ref objdmass, ref tmpdmass);
2109
2110 // fix all positions
2111 IntPtr g = d.BodyGetFirstGeom(Body);
2112 while (g != IntPtr.Zero)
2113 { 1871 {
2114 thispos = d.GeomGetOffsetPosition(g); 1872 // daughter prim, do Geom set
2115 thispos.X -= objdmass.c.X; 1873 d.GeomSetQuaternion(prim_geom, ref myrot);
2116 thispos.Y -= objdmass.c.Y;
2117 thispos.Z -= objdmass.c.Z;
2118 d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
2119 g = d.dBodyGetNextGeom(g);
2120 } 1874 }
2121 1875
2122 d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos); 1876 resetCollisionAccounting();
2123 // get current object position and rotation 1877 m_taintrot = _orientation;
2124 dobjpos = d.BodyGetPosition(Body);
2125
2126 d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
2127 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
2128 d.BodySetMass(Body, ref objdmass);
2129 _mass = objdmass.mass;
2130 } 1878 }
2131 1879
2132 1880 private void resetCollisionAccounting()
2133 #region Mass Calculation
2134
2135 private void UpdatePrimBodyData()
2136 { 1881 {
2137 primMass = m_density * primVolume; 1882 m_collisionscore = 0;
2138 1883 m_interpenetrationcount = 0;
2139 if (primMass <= 0) 1884 m_disabled = false;
2140 primMass = 0.0001f;//ckrinke: Mass must be greater then zero. 1885 }
2141 if (primMass > _parent_scene.maximumMassObject)
2142 primMass = _parent_scene.maximumMassObject;
2143
2144 _mass = primMass; // just in case
2145
2146 d.MassSetBoxTotal(out primdMass, primMass, m_OBB.X, m_OBB.Y, m_OBB.Z);
2147
2148 d.MassTranslate(ref primdMass,
2149 m_OBBOffset.X,
2150 m_OBBOffset.Y,
2151 m_OBBOffset.Z);
2152
2153 primOOBradiusSQ = m_OBB.LengthSquared();
2154 1886
2155 if (_triMeshData != IntPtr.Zero) 1887 /// <summary>
1888 /// Change prim in response to a disable taint.
1889 /// </summary>
1890 private void changedisable()
1891 {
1892 m_disabled = true;
1893 if (Body != IntPtr.Zero)
2156 { 1894 {
2157 float pc = m_physCost; 1895 d.BodyDisable(Body);
2158 float psf = primOOBradiusSQ; 1896 Body = IntPtr.Zero;
2159 psf *= 1.33f * .2f;
2160 pc *= psf;
2161 if (pc < 0.1f)
2162 pc = 0.1f;
2163
2164 m_physCost = pc;
2165 } 1897 }
2166 else
2167 m_physCost = 0.1f;
2168 1898
2169 m_streamCost = 1.0f; 1899 m_taintdisable = false;
2170 } 1900 }
2171 1901
2172 #endregion
2173
2174
2175 /// <summary> 1902 /// <summary>
2176 /// Add a child prim to this parent prim. 1903 /// Change prim in response to a physics status taint
2177 /// </summary> 1904 /// </summary>
2178 /// <param name="prim">Child prim</param> 1905 private void changePhysicsStatus()
2179 // I'm the parent
2180 // prim is the child
2181 public void ParentPrim(OdePrim prim)
2182 { 1906 {
2183 //Console.WriteLine("ParentPrim " + m_primName); 1907 if (IsPhysical)
2184 if (this.m_localID != prim.m_localID)
2185 { 1908 {
2186 DestroyBody(); // for now we need to rebuil entire object on link change 1909 if (Body == IntPtr.Zero)
2187
2188 lock (childrenPrim)
2189 { 1910 {
2190 // adopt the prim 1911 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
2191 if (!childrenPrim.Contains(prim))
2192 childrenPrim.Add(prim);
2193
2194 // see if this prim has kids and adopt them also
2195 // should not happen for now
2196 foreach (OdePrim prm in prim.childrenPrim)
2197 { 1912 {
2198 if (!childrenPrim.Contains(prm)) 1913 changeshape();
2199 { 1914 }
2200 if (prm.Body != IntPtr.Zero) 1915 else
2201 { 1916 {
2202 if (prm.prim_geom != IntPtr.Zero) 1917 enableBody();
2203 d.GeomSetBody(prm.prim_geom, IntPtr.Zero);
2204 if (prm.Body != prim.Body)
2205 prm.DestroyBody(); // don't loose bodies around
2206 prm.Body = IntPtr.Zero;
2207 }
2208
2209 childrenPrim.Add(prm);
2210 prm._parent = this;
2211 }
2212 } 1918 }
2213 } 1919 }
2214 //Remove old children from the prim
2215 prim.childrenPrim.Clear();
2216
2217 if (prim.Body != IntPtr.Zero)
2218 {
2219 if (prim.prim_geom != IntPtr.Zero)
2220 d.GeomSetBody(prim.prim_geom, IntPtr.Zero);
2221 prim.DestroyBody(); // don't loose bodies around
2222 prim.Body = IntPtr.Zero;
2223 }
2224
2225 prim.childPrim = true;
2226 prim._parent = this;
2227
2228 MakeBody(); // full nasty reconstruction
2229 }
2230 }
2231
2232 private void UpdateChildsfromgeom()
2233 {
2234 if (childrenPrim.Count > 0)
2235 {
2236 foreach (OdePrim prm in childrenPrim)
2237 prm.UpdateDataFromGeom();
2238 } 1920 }
2239 } 1921 else
2240
2241 private void UpdateDataFromGeom()
2242 {
2243 if (prim_geom != IntPtr.Zero)
2244 { 1922 {
2245 d.Quaternion qtmp; 1923 if (Body != IntPtr.Zero)
2246 d.GeomCopyQuaternion(prim_geom, out qtmp); 1924 {
2247 _orientation.X = qtmp.X; 1925 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
2248 _orientation.Y = qtmp.Y; 1926 {
2249 _orientation.Z = qtmp.Z; 1927 RemoveGeom();
2250 _orientation.W = qtmp.W;
2251
2252// Debug
2253 float qlen = _orientation.Length();
2254 if (qlen > 1.01f || qlen < 0.99)
2255 m_log.WarnFormat("[PHYSICS]: Got nonnorm quaternion from geom in Object {0} norm {1}", Name, qlen);
2256//
2257 _orientation.Normalize();
2258
2259 d.Vector3 lpos = d.GeomGetPosition(prim_geom);
2260 _position.X = lpos.X;
2261 _position.Y = lpos.Y;
2262 _position.Z = lpos.Z;
2263 }
2264 }
2265
2266 private void ChildDelink(OdePrim odePrim, bool remakebodies)
2267 {
2268 // Okay, we have a delinked child.. destroy all body and remake
2269 if (odePrim != this && !childrenPrim.Contains(odePrim))
2270 return;
2271 1928
2272 DestroyBody(); 1929//Console.WriteLine("changePhysicsStatus for " + Name);
1930 changeadd();
1931 }
2273 1932
2274 if (odePrim == this) // delinking the root prim 1933 if (childPrim)
2275 {
2276 OdePrim newroot = null;
2277 lock (childrenPrim)
2278 {
2279 if (childrenPrim.Count > 0)
2280 { 1934 {
2281 newroot = childrenPrim[0]; 1935 if (_parent != null)
2282 childrenPrim.RemoveAt(0);
2283 foreach (OdePrim prm in childrenPrim)
2284 { 1936 {
2285 newroot.childrenPrim.Add(prm); 1937 OdePrim parent = (OdePrim)_parent;
1938 parent.ChildDelink(this);
2286 } 1939 }
2287 childrenPrim.Clear();
2288 } 1940 }
2289 if (newroot != null) 1941 else
2290 { 1942 {
2291 newroot.childPrim = false; 1943 disableBody();
2292 newroot._parent = null;
2293 if (remakebodies)
2294 newroot.MakeBody();
2295 } 1944 }
2296 } 1945 }
2297 } 1946 }
2298 1947
2299 else 1948 changeSelectedStatus();
2300 { 1949
2301 lock (childrenPrim) 1950 resetCollisionAccounting();
2302 { 1951 m_taintPhysics = IsPhysical;
2303 childrenPrim.Remove(odePrim);
2304 odePrim.childPrim = false;
2305 odePrim._parent = null;
2306 // odePrim.UpdateDataFromGeom();
2307 if (remakebodies)
2308 odePrim.MakeBody();
2309 }
2310 }
2311 if (remakebodies)
2312 MakeBody();
2313 } 1952 }
2314 1953
2315 protected void ChildRemove(OdePrim odePrim, bool reMakeBody) 1954 /// <summary>
1955 /// Change prim in response to a size taint.
1956 /// </summary>
1957 private void changesize()
2316 { 1958 {
2317 // Okay, we have a delinked child.. destroy all body and remake 1959#if SPAM
2318 if (odePrim != this && !childrenPrim.Contains(odePrim)) 1960 m_log.DebugFormat("[ODE PRIM]: Called changesize");
2319 return; 1961#endif
2320 1962
2321 DestroyBody(); 1963 if (_size.X <= 0) _size.X = 0.01f;
1964 if (_size.Y <= 0) _size.Y = 0.01f;
1965 if (_size.Z <= 0) _size.Z = 0.01f;
2322 1966
2323 if (odePrim == this) 1967 //kill body to rebuild
1968 if (IsPhysical && Body != IntPtr.Zero)
2324 { 1969 {
2325 OdePrim newroot = null; 1970 if (childPrim)
2326 lock (childrenPrim)
2327 { 1971 {
2328 if (childrenPrim.Count > 0) 1972 if (_parent != null)
2329 {
2330 newroot = childrenPrim[0];
2331 childrenPrim.RemoveAt(0);
2332 foreach (OdePrim prm in childrenPrim)
2333 {
2334 newroot.childrenPrim.Add(prm);
2335 }
2336 childrenPrim.Clear();
2337 }
2338 if (newroot != null)
2339 { 1973 {
2340 newroot.childPrim = false; 1974 OdePrim parent = (OdePrim)_parent;
2341 newroot._parent = null; 1975 parent.ChildDelink(this);
2342 newroot.MakeBody();
2343 } 1976 }
2344 } 1977 }
2345 if (reMakeBody) 1978 else
2346 MakeBody();
2347 return;
2348 }
2349 else
2350 {
2351 lock (childrenPrim)
2352 { 1979 {
2353 childrenPrim.Remove(odePrim); 1980 disableBody();
2354 odePrim.childPrim = false;
2355 odePrim._parent = null;
2356 if (reMakeBody)
2357 odePrim.MakeBody();
2358 } 1981 }
2359 } 1982 }
2360 MakeBody();
2361 }
2362 1983
2363 #region changes 1984 if (d.SpaceQuery(m_targetSpace, prim_geom))
1985 {
1986// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1987 d.SpaceRemove(m_targetSpace, prim_geom);
1988 }
2364 1989
2365 private void changeadd() 1990 RemoveGeom();
2366 {
2367 }
2368 1991
2369 private void changeAngularLock(Vector3 newLock) 1992 // we don't need to do space calculation because the client sends a position update also.
2370 { 1993
2371 // do we have a Physical object? 1994 IMesh mesh = null;
2372 if (Body != IntPtr.Zero) 1995
1996 // Construction of new prim
1997 if (_parent_scene.needsMeshing(_pbs))
2373 { 1998 {
2374 //Check that we have a Parent 1999 float meshlod = _parent_scene.meshSculptLOD;
2375 //If we have a parent then we're not authorative here 2000
2376 if (_parent == null) 2001 if (IsPhysical)
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))
2377 { 2006 {
2378 if (!newLock.ApproxEquals(Vector3.One, 0f)) 2007 mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical);
2379 { 2008 if (mesh == null)
2380 createAMotor(newLock); 2009 CheckMeshAsset();
2381 }
2382 else
2383 {
2384 if (Amotor != IntPtr.Zero)
2385 {
2386 d.JointDestroy(Amotor);
2387 Amotor = IntPtr.Zero;
2388 }
2389 }
2390 } 2010 }
2011
2391 } 2012 }
2392 // Store this for later in case we get turned into a separate body
2393 m_angularlock = newLock;
2394 }
2395 2013
2396 private void changeLink(OdePrim NewParent) 2014 CreateGeom(m_targetSpace, mesh);
2397 { 2015 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2398 if (_parent == null && NewParent != null) 2016 d.Quaternion myrot = new d.Quaternion();
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)
2399 { 2025 {
2400 NewParent.ParentPrim(this); 2026 // Re creates body on size.
2027 // EnableBody also does setMass()
2028 enableBody();
2029 d.BodyEnable(Body);
2401 } 2030 }
2402 else if (_parent != null) 2031
2032 changeSelectedStatus();
2033
2034 if (childPrim)
2403 { 2035 {
2404 if (_parent is OdePrim) 2036 if (_parent is OdePrim)
2405 { 2037 {
2406 if (NewParent != _parent) 2038 OdePrim parent = (OdePrim)_parent;
2407 { 2039 parent.ChildSetGeom(this);
2408 (_parent as OdePrim).ChildDelink(this, false); // for now...
2409 childPrim = false;
2410
2411 if (NewParent != null)
2412 {
2413 NewParent.ParentPrim(this);
2414 }
2415 }
2416 } 2040 }
2417 } 2041 }
2418 _parent = NewParent; 2042 resetCollisionAccounting();
2043 m_taintsize = _size;
2419 } 2044 }
2420 2045
2421 2046 /// <summary>
2422 private void Stop() 2047 /// Change prim in response to a float on water taint.
2048 /// </summary>
2049 /// <param name="timestep"></param>
2050 private void changefloatonwater()
2423 { 2051 {
2424 if (!childPrim) 2052 m_collidesWater = m_taintCollidesWater;
2425 {
2426 m_force = Vector3.Zero;
2427 m_forceacc = Vector3.Zero;
2428 m_angularForceacc = Vector3.Zero;
2429 _torque = Vector3.Zero;
2430 _velocity = Vector3.Zero;
2431 _acceleration = Vector3.Zero;
2432 m_rotationalVelocity = Vector3.Zero;
2433 _target_velocity = Vector3.Zero;
2434 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
2435 m_vehicle.Stop();
2436 2053
2437 _zeroFlag = false; 2054 if (m_collidesWater)
2438 base.RequestPhysicsterseUpdate(); 2055 {
2056 m_collisionFlags |= CollisionCategories.Water;
2439 } 2057 }
2440 2058 else
2441 if (Body != IntPtr.Zero)
2442 { 2059 {
2443 d.BodySetForce(Body, 0f, 0f, 0f); 2060 m_collisionFlags &= ~CollisionCategories.Water;
2444 d.BodySetTorque(Body, 0f, 0f, 0f);
2445 d.BodySetLinearVel(Body, 0f, 0f, 0f);
2446 d.BodySetAngularVel(Body, 0f, 0f, 0f);
2447 } 2061 }
2062
2063 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2448 } 2064 }
2449 2065
2450 private void changePhantomStatus(bool newval) 2066 /// <summary>
2067 /// Change prim in response to a shape taint.
2068 /// </summary>
2069 private void changeshape()
2451 { 2070 {
2452 m_isphantom = newval; 2071 m_taintshape = false;
2453 2072
2454 UpdateCollisionCatFlags(); 2073 // Cleanup of old prim geometry and Bodies
2455 ApplyCollisionCatFlags(); 2074 if (IsPhysical && Body != IntPtr.Zero)
2456 } 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 }
2457 2089
2458/* not in use 2090 RemoveGeom();
2459 internal void ChildSelectedChange(bool childSelect)
2460 {
2461 if(childPrim)
2462 return;
2463 2091
2464 if (childSelect == m_isSelected) 2092 // we don't need to do space calculation because the client sends a position update also.
2465 return; 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
2098 IMesh mesh = null;
2466 2099
2467 if (childSelect) 2100
2101 if (_parent_scene.needsMeshing(_pbs))
2468 { 2102 {
2469 DoSelectedStatus(true); 2103 // Don't need to re-enable body.. it's done in CreateMesh
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();
2470 } 2113 }
2471 2114
2472 else 2115 CreateGeom(m_targetSpace, mesh);
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)
2473 { 2127 {
2474 foreach (OdePrim prm in childrenPrim) 2128 // Re creates body on size.
2129 // EnableBody also does setMass()
2130 enableBody();
2131 if (Body != IntPtr.Zero)
2475 { 2132 {
2476 if (prm.m_isSelected) 2133 d.BodyEnable(Body);
2477 return;
2478 } 2134 }
2479 DoSelectedStatus(false);
2480 } 2135 }
2481 }
2482*/
2483 private void changeSelectedStatus(bool newval)
2484 {
2485 if (m_lastdoneSelected == newval)
2486 return;
2487 2136
2488 m_lastdoneSelected = newval; 2137 changeSelectedStatus();
2489 DoSelectedStatus(newval);
2490 }
2491 2138
2492 private void CheckDelaySelect() 2139 if (childPrim)
2493 {
2494 if (m_delaySelect)
2495 { 2140 {
2496 DoSelectedStatus(m_isSelected); 2141 if (_parent is OdePrim)
2142 {
2143 OdePrim parent = (OdePrim)_parent;
2144 parent.ChildSetGeom(this);
2145 }
2497 } 2146 }
2147
2148 resetCollisionAccounting();
2149// m_taintshape = false;
2498 } 2150 }
2499 2151
2500 private void DoSelectedStatus(bool newval) 2152 /// <summary>
2153 /// Change prim in response to an add force taint.
2154 /// </summary>
2155 private void changeAddForce()
2501 { 2156 {
2502 m_isSelected = newval; 2157 if (!m_isSelected)
2503 Stop();
2504
2505 if (newval)
2506 { 2158 {
2507 if (!childPrim && Body != IntPtr.Zero) 2159 lock (m_forcelist)
2508 d.BodyDisable(Body);
2509
2510 if (m_delaySelect || m_isphysical)
2511 { 2160 {
2512 m_collisionCategories = CollisionCategories.Selected; 2161 //m_log.Info("[PHYSICS]: dequeing forcelist");
2513 m_collisionFlags = 0; 2162 if (IsPhysical)
2514
2515 if (!childPrim)
2516 { 2163 {
2517 foreach (OdePrim prm in childrenPrim) 2164 Vector3 iforce = Vector3.Zero;
2165 int i = 0;
2166 try
2518 { 2167 {
2519 prm.m_collisionCategories = m_collisionCategories; 2168 for (i = 0; i < m_forcelist.Count; i++)
2520 prm.m_collisionFlags = m_collisionFlags;
2521
2522 if (prm.prim_geom != null)
2523 { 2169 {
2524 2170
2525 if (prm.m_NoColide) 2171 iforce = iforce + (m_forcelist[i] * 100);
2526 {
2527 d.GeomSetCategoryBits(prm.prim_geom, 0);
2528 d.GeomSetCollideBits(prm.prim_geom, 0);
2529 }
2530 else
2531 {
2532 d.GeomSetCategoryBits(prm.prim_geom, (uint)m_collisionCategories);
2533 d.GeomSetCollideBits(prm.prim_geom, (uint)m_collisionFlags);
2534 }
2535 } 2172 }
2536 prm.m_delaySelect = false;
2537 } 2173 }
2538 } 2174 catch (IndexOutOfRangeException)
2539// else if (_parent != null)
2540// ((OdePrim)_parent).ChildSelectedChange(true);
2541
2542
2543 if (prim_geom != null)
2544 {
2545 if (m_NoColide)
2546 { 2175 {
2547 d.GeomSetCategoryBits(prim_geom, 0); 2176 m_forcelist = new List<Vector3>();
2548 d.GeomSetCollideBits(prim_geom, 0); 2177 m_collisionscore = 0;
2549 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) 2178 m_interpenetrationcount = 0;
2550 { 2179 m_taintforce = false;
2551 d.GeomSetCategoryBits(collide_geom, 0); 2180 return;
2552 d.GeomSetCollideBits(collide_geom, 0);
2553 }
2554
2555 } 2181 }
2556 else 2182 catch (ArgumentOutOfRangeException)
2557 { 2183 {
2558 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); 2184 m_forcelist = new List<Vector3>();
2559 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); 2185 m_collisionscore = 0;
2560 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) 2186 m_interpenetrationcount = 0;
2561 { 2187 m_taintforce = false;
2562 d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories); 2188 return;
2563 d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags);
2564 }
2565 } 2189 }
2566 }
2567
2568 m_delaySelect = false;
2569 }
2570 else if(!m_isphysical)
2571 {
2572 m_delaySelect = true;
2573 }
2574 }
2575 else
2576 {
2577 if (!childPrim)
2578 {
2579 if (Body != IntPtr.Zero && !m_disabled)
2580 d.BodyEnable(Body); 2190 d.BodyEnable(Body);
2191 d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z);
2192 }
2193 m_forcelist.Clear();
2581 } 2194 }
2582// else if (_parent != null)
2583// ((OdePrim)_parent).ChildSelectedChange(false);
2584 2195
2585 UpdateCollisionCatFlags(); 2196 m_collisionscore = 0;
2586 ApplyCollisionCatFlags(); 2197 m_interpenetrationcount = 0;
2587
2588 m_delaySelect = false;
2589 } 2198 }
2590 2199
2591 resetCollisionAccounting(); 2200 m_taintforce = false;
2592 } 2201 }
2593 2202
2594 private void changePosition(Vector3 newPos) 2203 /// <summary>
2204 /// Change prim in response to a torque taint.
2205 /// </summary>
2206 private void changeSetTorque()
2595 { 2207 {
2596 CheckDelaySelect(); 2208 if (!m_isSelected)
2597 if (m_isphysical)
2598 { 2209 {
2599 if (childPrim) // inertia is messed, must rebuild 2210 if (IsPhysical && Body != IntPtr.Zero)
2600 { 2211 {
2601 if (m_building) 2212 d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z);
2602 {
2603 _position = newPos;
2604 }
2605
2606 else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero)
2607 {
2608 FixInertia(newPos);
2609 if (!d.BodyIsEnabled(Body))
2610 d.BodyEnable(Body);
2611 }
2612 }
2613 else
2614 {
2615 if (_position != newPos)
2616 {
2617 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2618 _position = newPos;
2619 }
2620 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2621 d.BodyEnable(Body);
2622 } 2213 }
2623 } 2214 }
2624 else
2625 {
2626 if (prim_geom != IntPtr.Zero)
2627 {
2628 if (newPos != _position)
2629 {
2630 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2631 _position = newPos;
2632 2215
2633 m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); 2216 m_taintTorque = Vector3.Zero;
2634 }
2635 }
2636 }
2637 givefakepos--;
2638 if (givefakepos < 0)
2639 givefakepos = 0;
2640// changeSelectedStatus();
2641 resetCollisionAccounting();
2642 } 2217 }
2643 2218
2644 private void changeOrientation(Quaternion newOri) 2219 /// <summary>
2220 /// Change prim in response to an angular force taint.
2221 /// </summary>
2222 private void changeAddAngularForce()
2645 { 2223 {
2646 CheckDelaySelect(); 2224 if (!m_isSelected)
2647 if (m_isphysical)
2648 { 2225 {
2649 if (childPrim) // inertia is messed, must rebuild 2226 lock (m_angularforcelist)
2650 {
2651 if (m_building)
2652 {
2653 _orientation = newOri;
2654 }
2655/*
2656 else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero)
2657 {
2658 FixInertia(_position, newOri);
2659 if (!d.BodyIsEnabled(Body))
2660 d.BodyEnable(Body);
2661 }
2662*/
2663 }
2664 else
2665 { 2227 {
2666 if (newOri != _orientation) 2228 //m_log.Info("[PHYSICS]: dequeing forcelist");
2229 if (IsPhysical)
2667 { 2230 {
2668 d.Quaternion myrot = new d.Quaternion(); 2231 Vector3 iforce = Vector3.Zero;
2669 myrot.X = newOri.X; 2232 for (int i = 0; i < m_angularforcelist.Count; i++)
2670 myrot.Y = newOri.Y; 2233 {
2671 myrot.Z = newOri.Z; 2234 iforce = iforce + (m_angularforcelist[i] * 100);
2672 myrot.W = newOri.W; 2235 }
2673 d.GeomSetQuaternion(prim_geom, ref myrot);
2674 _orientation = newOri;
2675 if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
2676 createAMotor(m_angularlock);
2677 }
2678 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2679 d.BodyEnable(Body); 2236 d.BodyEnable(Body);
2680 } 2237 d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z);
2681 } 2238
2682 else
2683 {
2684 if (prim_geom != IntPtr.Zero)
2685 {
2686 if (newOri != _orientation)
2687 {
2688 d.Quaternion myrot = new d.Quaternion();
2689 myrot.X = newOri.X;
2690 myrot.Y = newOri.Y;
2691 myrot.Z = newOri.Z;
2692 myrot.W = newOri.W;
2693 d.GeomSetQuaternion(prim_geom, ref myrot);
2694 _orientation = newOri;
2695 } 2239 }
2240 m_angularforcelist.Clear();
2696 } 2241 }
2697 }
2698 givefakeori--;
2699 if (givefakeori < 0)
2700 givefakeori = 0;
2701 resetCollisionAccounting();
2702 }
2703 2242
2704 private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri) 2243 m_collisionscore = 0;
2705 { 2244 m_interpenetrationcount = 0;
2706 CheckDelaySelect();
2707 if (m_isphysical)
2708 {
2709 if (childPrim && m_building) // inertia is messed, must rebuild
2710 {
2711 _position = newPos;
2712 _orientation = newOri;
2713 }
2714 else
2715 {
2716 if (newOri != _orientation)
2717 {
2718 d.Quaternion myrot = new d.Quaternion();
2719 myrot.X = newOri.X;
2720 myrot.Y = newOri.Y;
2721 myrot.Z = newOri.Z;
2722 myrot.W = newOri.W;
2723 d.GeomSetQuaternion(prim_geom, ref myrot);
2724 _orientation = newOri;
2725 if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
2726 createAMotor(m_angularlock);
2727 }
2728 if (_position != newPos)
2729 {
2730 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2731 _position = newPos;
2732 }
2733 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2734 d.BodyEnable(Body);
2735 }
2736 } 2245 }
2737 else
2738 {
2739 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
2740 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
2741
2742 if (prim_geom != IntPtr.Zero)
2743 {
2744 if (newOri != _orientation)
2745 {
2746 d.Quaternion myrot = new d.Quaternion();
2747 myrot.X = newOri.X;
2748 myrot.Y = newOri.Y;
2749 myrot.Z = newOri.Z;
2750 myrot.W = newOri.W;
2751 d.GeomSetQuaternion(prim_geom, ref myrot);
2752 _orientation = newOri;
2753 }
2754 2246
2755 if (newPos != _position) 2247 m_taintaddangularforce = false;
2756 {
2757 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2758 _position = newPos;
2759
2760 m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace);
2761 }
2762 }
2763 }
2764 givefakepos--;
2765 if (givefakepos < 0)
2766 givefakepos = 0;
2767 givefakeori--;
2768 if (givefakeori < 0)
2769 givefakeori = 0;
2770 resetCollisionAccounting();
2771 } 2248 }
2772 2249
2773 private void changeDisable(bool disable) 2250 /// <summary>
2251 /// Change prim in response to a velocity taint.
2252 /// </summary>
2253 private void changevelocity()
2774 { 2254 {
2775 if (disable) 2255 if (!m_isSelected)
2776 {
2777 if (!m_disabled)
2778 disableBodySoft();
2779 }
2780 else
2781 { 2256 {
2782 if (m_disabled) 2257 // Not sure exactly why this sleep is here, but from experimentation it appears to stop an avatar
2783 enableBodySoft(); 2258 // walking through a default rez size prim if it keeps kicking it around - justincc.
2784 } 2259 Thread.Sleep(20);
2785 }
2786 2260
2787 private void changePhysicsStatus(bool NewStatus) 2261 if (IsPhysical)
2788 {
2789 CheckDelaySelect();
2790
2791 m_isphysical = NewStatus;
2792
2793 if (!childPrim)
2794 {
2795 if (NewStatus)
2796 {
2797 if (Body == IntPtr.Zero)
2798 MakeBody();
2799 }
2800 else
2801 { 2262 {
2802 if (Body != IntPtr.Zero) 2263 if (Body != IntPtr.Zero)
2803 { 2264 {
2804 DestroyBody(); 2265 d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
2805 } 2266 }
2806 Stop();
2807 } 2267 }
2268
2269 //resetCollisionAccounting();
2808 } 2270 }
2809 2271
2810 resetCollisionAccounting(); 2272 m_taintVelocity = Vector3.Zero;
2811 } 2273 }
2812 2274
2813 private void changeSize(Vector3 newSize) 2275 internal void setPrimForRemoval()
2814 { 2276 {
2277 m_taintremove = true;
2815 } 2278 }
2816 2279
2817 private void changeShape(PrimitiveBaseShape newShape) 2280 public override bool Flying
2818 { 2281 {
2282 // no flying prims for you
2283 get { return false; }
2284 set { }
2819 } 2285 }
2820 2286
2821 private void changeAddPhysRep(ODEPhysRepData repData) 2287 public override bool IsColliding
2822 { 2288 {
2823 _size = repData.size; //?? 2289 get { return iscolliding; }
2824 _pbs = repData.pbs; 2290 set { iscolliding = value; }
2825 m_shapetype = repData.shapetype; 2291 }
2826
2827 m_mesh = repData.mesh;
2828
2829 m_assetID = repData.assetID;
2830 m_meshState = repData.meshState;
2831 2292
2832 m_hasOBB = repData.hasOBB; 2293 public override bool CollidingGround
2833 m_OBBOffset = repData.OBBOffset; 2294 {
2834 m_OBB = repData.OBB; 2295 get { return false; }
2296 set { return; }
2297 }
2835 2298
2836 primVolume = repData.volume; 2299 public override bool CollidingObj
2300 {
2301 get { return false; }
2302 set { return; }
2303 }
2837 2304
2838 CreateGeom(); 2305 public override bool ThrottleUpdates
2306 {
2307 get { return m_throttleUpdates; }
2308 set { m_throttleUpdates = value; }
2309 }
2839 2310
2840 if (prim_geom != IntPtr.Zero) 2311 public override bool Stopped
2841 { 2312 {
2842 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); 2313 get { return _zeroFlag; }
2843 d.Quaternion myrot = new d.Quaternion(); 2314 }
2844 myrot.X = _orientation.X;
2845 myrot.Y = _orientation.Y;
2846 myrot.Z = _orientation.Z;
2847 myrot.W = _orientation.W;
2848 d.GeomSetQuaternion(prim_geom, ref myrot);
2849 }
2850 2315
2851 if (!m_isphysical) 2316 public override Vector3 Position
2852 { 2317 {
2853 SetInStaticSpace(this); 2318 get { return _position; }
2854 UpdateCollisionCatFlags();
2855 ApplyCollisionCatFlags();
2856 }
2857 else
2858 MakeBody();
2859 2319
2860 if ((m_meshState & MeshState.NeedMask) != 0) 2320 set { _position = value;
2861 { 2321 //m_log.Info("[PHYSICS]: " + _position.ToString());
2862 repData.size = _size;
2863 repData.pbs = _pbs;
2864 repData.shapetype = m_shapetype;
2865 _parent_scene.m_meshWorker.RequestMesh(repData);
2866 } 2322 }
2867 } 2323 }
2868 2324
2869 private void changePhysRepData(ODEPhysRepData repData) 2325 public override Vector3 Size
2870 { 2326 {
2871 CheckDelaySelect(); 2327 get { return _size; }
2872 2328 set
2873 OdePrim parent = (OdePrim)_parent;
2874
2875 bool chp = childPrim;
2876
2877 if (chp)
2878 { 2329 {
2879 if (parent != null) 2330 if (value.IsFinite())
2880 { 2331 {
2881 parent.DestroyBody(); 2332 _size = value;
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);
2882 } 2338 }
2883 } 2339 }
2884 else 2340 }
2885 {
2886 DestroyBody();
2887 }
2888
2889 RemoveGeom();
2890
2891 _size = repData.size;
2892 _pbs = repData.pbs;
2893 m_shapetype = repData.shapetype;
2894
2895 m_mesh = repData.mesh;
2896
2897 m_assetID = repData.assetID;
2898 m_meshState = repData.meshState;
2899
2900 m_hasOBB = repData.hasOBB;
2901 m_OBBOffset = repData.OBBOffset;
2902 m_OBB = repData.OBB;
2903
2904 primVolume = repData.volume;
2905
2906 CreateGeom();
2907 2341
2908 if (prim_geom != IntPtr.Zero) 2342 public override float Mass
2909 { 2343 {
2910 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); 2344 get { return CalculateMass(); }
2911 d.Quaternion myrot = new d.Quaternion(); 2345 }
2912 myrot.X = _orientation.X;
2913 myrot.Y = _orientation.Y;
2914 myrot.Z = _orientation.Z;
2915 myrot.W = _orientation.W;
2916 d.GeomSetQuaternion(prim_geom, ref myrot);
2917 }
2918 2346
2919 if (m_isphysical) 2347 public override Vector3 Force
2348 {
2349 //get { return Vector3.Zero; }
2350 get { return m_force; }
2351 set
2920 { 2352 {
2921 if (chp) 2353 if (value.IsFinite())
2922 { 2354 {
2923 if (parent != null) 2355 m_force = value;
2924 {
2925 parent.MakeBody();
2926 }
2927 } 2356 }
2928 else 2357 else
2929 MakeBody(); 2358 {
2930 } 2359 m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name);
2931 else 2360 }
2932 {
2933 SetInStaticSpace(this);
2934 UpdateCollisionCatFlags();
2935 ApplyCollisionCatFlags();
2936 } 2361 }
2362 }
2937 2363
2938 resetCollisionAccounting(); 2364 public override int VehicleType
2365 {
2366 get { return (int)m_vehicle.Type; }
2367 set { m_vehicle.ProcessTypeChange((Vehicle)value); }
2368 }
2939 2369
2940 if ((m_meshState & MeshState.NeedMask) != 0) 2370 public override void VehicleFloatParam(int param, float value)
2941 { 2371 {
2942 repData.size = _size; 2372 m_vehicle.ProcessFloatVehicleParam((Vehicle) param, value);
2943 repData.pbs = _pbs;
2944 repData.shapetype = m_shapetype;
2945 _parent_scene.m_meshWorker.RequestMesh(repData);
2946 }
2947 } 2373 }
2948 2374
2949 private void changeFloatOnWater(bool newval) 2375 public override void VehicleVectorParam(int param, Vector3 value)
2950 { 2376 {
2951 m_collidesWater = newval; 2377 m_vehicle.ProcessVectorVehicleParam((Vehicle) param, value);
2378 }
2952 2379
2953 UpdateCollisionCatFlags(); 2380 public override void VehicleRotationParam(int param, Quaternion rotation)
2954 ApplyCollisionCatFlags(); 2381 {
2382 m_vehicle.ProcessRotationVehicleParam((Vehicle) param, rotation);
2955 } 2383 }
2956 2384
2957 private void changeSetTorque(Vector3 newtorque) 2385 public override void VehicleFlags(int param, bool remove)
2958 { 2386 {
2959 if (!m_isSelected) 2387 m_vehicle.ProcessVehicleFlags(param, remove);
2960 { 2388 }
2961 if (m_isphysical && Body != IntPtr.Zero)
2962 {
2963 if (m_disabled)
2964 enableBodySoft();
2965 else if (!d.BodyIsEnabled(Body))
2966 d.BodyEnable(Body);
2967 2389
2968 } 2390 public override void SetVolumeDetect(int param)
2969 _torque = newtorque; 2391 {
2392 // We have to lock the scene here so that an entire simulate loop either uses volume detect for all
2393 // possible collisions with this prim or for none of them.
2394 lock (_parent_scene.OdeLock)
2395 {
2396 m_isVolumeDetect = (param != 0);
2970 } 2397 }
2971 } 2398 }
2972 2399
2973 private void changeForce(Vector3 force) 2400 public override Vector3 CenterOfMass
2974 { 2401 {
2975 m_force = force; 2402 get { return Vector3.Zero; }
2976 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2977 d.BodyEnable(Body);
2978 } 2403 }
2979 2404
2980 private void changeAddForce(Vector3 theforce) 2405 public override Vector3 GeometricCenter
2981 { 2406 {
2982 m_forceacc += theforce; 2407 get { return Vector3.Zero; }
2983 if (!m_isSelected)
2984 {
2985 lock (this)
2986 {
2987 //m_log.Info("[PHYSICS]: dequeing forcelist");
2988 if (m_isphysical && Body != IntPtr.Zero)
2989 {
2990 if (m_disabled)
2991 enableBodySoft();
2992 else if (!d.BodyIsEnabled(Body))
2993 d.BodyEnable(Body);
2994 }
2995 }
2996 m_collisionscore = 0;
2997 }
2998 } 2408 }
2999 2409
3000 // actually angular impulse 2410 public override PrimitiveBaseShape Shape
3001 private void changeAddAngularImpulse(Vector3 aimpulse)
3002 { 2411 {
3003 m_angularForceacc += aimpulse * m_invTimeStep; 2412 set
3004 if (!m_isSelected)
3005 { 2413 {
3006 lock (this) 2414 _pbs = value;
3007 { 2415 m_assetFailed = false;
3008 if (m_isphysical && Body != IntPtr.Zero) 2416 m_taintshape = true;
3009 {
3010 if (m_disabled)
3011 enableBodySoft();
3012 else if (!d.BodyIsEnabled(Body))
3013 d.BodyEnable(Body);
3014 }
3015 }
3016 m_collisionscore = 0;
3017 } 2417 }
3018 } 2418 }
3019 2419
3020 private void changevelocity(Vector3 newVel) 2420 public override Vector3 Velocity
3021 { 2421 {
3022 float len = newVel.LengthSquared(); 2422 get
3023 if (len > 100000.0f) // limit to 100m/s
3024 { 2423 {
3025 len = 100.0f / (float)Math.Sqrt(len); 2424 // Average previous velocity with the new one so
3026 newVel *= len; 2425 // client object interpolation works a 'little' better
3027 } 2426 if (_zeroFlag)
2427 return Vector3.Zero;
3028 2428
3029 if (!m_isSelected) 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 }
2435 set
3030 { 2436 {
3031 if (Body != IntPtr.Zero) 2437 if (value.IsFinite())
3032 { 2438 {
3033 if (m_disabled) 2439 _velocity = value;
3034 enableBodySoft();
3035 else if (!d.BodyIsEnabled(Body))
3036 d.BodyEnable(Body);
3037 2440
3038 d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z); 2441 m_taintVelocity = value;
2442 _parent_scene.AddPhysicsActorTaint(this);
2443 }
2444 else
2445 {
2446 m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name);
3039 } 2447 }
3040 //resetCollisionAccounting(); 2448
3041 } 2449 }
3042 _velocity = newVel;
3043 } 2450 }
3044 2451
3045 private void changeangvelocity(Vector3 newAngVel) 2452 public override Vector3 Torque
3046 { 2453 {
3047 float len = newAngVel.LengthSquared(); 2454 get
3048 if (len > 144.0f) // limit to 12rad/s
3049 { 2455 {
3050 len = 12.0f / (float)Math.Sqrt(len); 2456 if (!IsPhysical || Body == IntPtr.Zero)
3051 newAngVel *= len; 2457 return Vector3.Zero;
2458
2459 return _torque;
3052 } 2460 }
3053 2461
3054 if (!m_isSelected) 2462 set
3055 { 2463 {
3056 if (Body != IntPtr.Zero) 2464 if (value.IsFinite())
3057 { 2465 {
3058 if (m_disabled) 2466 m_taintTorque = value;
3059 enableBodySoft(); 2467 _parent_scene.AddPhysicsActorTaint(this);
3060 else if (!d.BodyIsEnabled(Body)) 2468 }
3061 d.BodyEnable(Body); 2469 else
3062 2470 {
3063 2471 m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name);
3064 d.BodySetAngularVel(Body, newAngVel.X, newAngVel.Y, newAngVel.Z);
3065 } 2472 }
3066 //resetCollisionAccounting();
3067 } 2473 }
3068 m_rotationalVelocity = newAngVel;
3069 } 2474 }
3070 2475
3071 private void changeVolumedetetion(bool newVolDtc) 2476 public override float CollisionScore
3072 {
3073 m_isVolumeDetect = newVolDtc;
3074 m_fakeisVolumeDetect = newVolDtc;
3075 UpdateCollisionCatFlags();
3076 ApplyCollisionCatFlags();
3077 }
3078
3079 protected void changeBuilding(bool newbuilding)
3080 { 2477 {
3081 // Check if we need to do anything 2478 get { return m_collisionscore; }
3082 if (newbuilding == m_building) 2479 set { m_collisionscore = value; }
3083 return;
3084
3085 if ((bool)newbuilding)
3086 {
3087 m_building = true;
3088 if (!childPrim)
3089 DestroyBody();
3090 }
3091 else
3092 {
3093 m_building = false;
3094 CheckDelaySelect();
3095 if (!childPrim)
3096 MakeBody();
3097 }
3098 if (!childPrim && childrenPrim.Count > 0)
3099 {
3100 foreach (OdePrim prm in childrenPrim)
3101 prm.changeBuilding(m_building); // call directly
3102 }
3103 } 2480 }
3104 2481
3105 public void changeSetVehicle(VehicleData vdata) 2482 public override bool Kinematic
3106 { 2483 {
3107 if (m_vehicle == null) 2484 get { return false; }
3108 m_vehicle = new ODEDynamics(this); 2485 set { }
3109 m_vehicle.DoSetVehicle(vdata);
3110 } 2486 }
3111 2487
3112 private void changeVehicleType(int value) 2488 public override Quaternion Orientation
3113 { 2489 {
3114 if (value == (int)Vehicle.TYPE_NONE) 2490 get { return _orientation; }
3115 { 2491 set
3116 if (m_vehicle != null)
3117 m_vehicle = null;
3118 }
3119 else
3120 { 2492 {
3121 if (m_vehicle == null) 2493 if (QuaternionIsFinite(value))
3122 m_vehicle = new ODEDynamics(this); 2494 _orientation = value;
3123 2495 else
3124 m_vehicle.ProcessTypeChange((Vehicle)value); 2496 m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name);
3125 } 2497 }
3126 } 2498 }
3127 2499
3128 private void changeVehicleFloatParam(strVehicleFloatParam fp) 2500 private static bool QuaternionIsFinite(Quaternion q)
3129 { 2501 {
3130 if (m_vehicle == null) 2502 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
3131 return; 2503 return false;
3132 2504 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
3133 m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value); 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;
3134 } 2511 }
3135 2512
3136 private void changeVehicleVectorParam(strVehicleVectorParam vp) 2513 public override Vector3 Acceleration
3137 { 2514 {
3138 if (m_vehicle == null) 2515 get { return _acceleration; }
3139 return; 2516 set { _acceleration = value; }
3140 m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value);
3141 } 2517 }
3142 2518
3143 private void changeVehicleRotationParam(strVehicleQuatParam qp) 2519 public override void AddForce(Vector3 force, bool pushforce)
3144 { 2520 {
3145 if (m_vehicle == null) 2521 if (force.IsFinite())
3146 return; 2522 {
3147 m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value); 2523 lock (m_forcelist)
3148 } 2524 m_forcelist.Add(force);
3149 2525
3150 private void changeVehicleFlags(strVehicleBoolParam bp) 2526 m_taintforce = true;
3151 { 2527 }
3152 if (m_vehicle == null) 2528 else
3153 return; 2529 {
3154 m_vehicle.ProcessVehicleFlags(bp.param, bp.value); 2530 m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name);
2531 }
2532 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
3155 } 2533 }
3156 2534
3157 private void changeBuoyancy(float b) 2535 public override void AddAngularForce(Vector3 force, bool pushforce)
3158 { 2536 {
3159 m_buoyancy = b; 2537 if (force.IsFinite())
2538 {
2539 m_angularforcelist.Add(force);
2540 m_taintaddangularforce = true;
2541 }
2542 else
2543 {
2544 m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name);
2545 }
3160 } 2546 }
3161 2547
3162 private void changePIDTarget(Vector3 trg) 2548 public override Vector3 RotationalVelocity
3163 { 2549 {
3164 m_PIDTarget = trg; 2550 get
3165 } 2551 {
2552 Vector3 pv = Vector3.Zero;
2553 if (_zeroFlag)
2554 return pv;
2555 m_lastUpdateSent = false;
3166 2556
3167 private void changePIDTau(float tau) 2557 if (m_rotationalVelocity.ApproxEquals(pv, 0.2f))
3168 { 2558 return pv;
3169 m_PIDTau = tau; 2559
2560 return m_rotationalVelocity;
2561 }
2562 set
2563 {
2564 if (value.IsFinite())
2565 {
2566 m_rotationalVelocity = value;
2567 }
2568 else
2569 {
2570 m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name);
2571 }
2572 }
3170 } 2573 }
3171 2574
3172 private void changePIDActive(bool val) 2575 public override void CrossingFailure()
3173 { 2576 {
3174 m_usePID = val; 2577 m_crossingfailures++;
2578 if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
2579 {
2580 base.RaiseOutOfBounds(_position);
2581 return;
2582 }
2583 else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
2584 {
2585 m_log.Warn("[PHYSICS]: Too many crossing failures for: " + Name);
2586 }
3175 } 2587 }
3176 2588
3177 private void changePIDHoverHeight(float val) 2589 public override float Buoyancy
3178 { 2590 {
3179 m_PIDHoverHeight = val; 2591 get { return m_buoyancy; }
3180 if (val == 0) 2592 set { m_buoyancy = value; }
3181 m_useHoverPID = false;
3182 } 2593 }
3183 2594
3184 private void changePIDHoverType(PIDHoverType type) 2595 public override void link(PhysicsActor obj)
3185 { 2596 {
3186 m_PIDHoverType = type; 2597 m_taintparent = obj;
3187 } 2598 }
3188 2599
3189 private void changePIDHoverTau(float tau) 2600 public override void delink()
3190 { 2601 {
3191 m_PIDHoverTau = tau; 2602 m_taintparent = null;
3192 } 2603 }
3193 2604
3194 private void changePIDHoverActive(bool active) 2605 public override void LockAngularMotion(Vector3 axis)
3195 { 2606 {
3196 m_useHoverPID = active; 2607 // reverse the zero/non zero values for ODE.
2608 if (axis.IsFinite())
2609 {
2610 axis.X = (axis.X > 0) ? 1f : 0f;
2611 axis.Y = (axis.Y > 0) ? 1f : 0f;
2612 axis.Z = (axis.Z > 0) ? 1f : 0f;
2613 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
2614 m_taintAngularLock = axis;
2615 }
2616 else
2617 {
2618 m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name);
2619 }
3197 } 2620 }
3198 2621
3199 #endregion 2622 internal void UpdatePositionAndVelocity()
3200
3201 public void Move()
3202 { 2623 {
3203 if (!childPrim && m_isphysical && Body != IntPtr.Zero && 2624 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
3204 !m_disabled && !m_isSelected && !m_building && !m_outbounds) 2625 if (_parent == null)
3205 { 2626 {
3206 if (!d.BodyIsEnabled(Body)) 2627 Vector3 pv = Vector3.Zero;
3207 { 2628 bool lastZeroFlag = _zeroFlag;
3208 // let vehicles sleep 2629 float m_minvelocity = 0;
3209 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) 2630 if (Body != (IntPtr)0) // FIXME -> or if it is a joint
3210 return; 2631 {
3211 2632 d.Vector3 vec = d.BodyGetPosition(Body);
3212 if (++bodydisablecontrol < 20) 2633 d.Quaternion ori = d.BodyGetQuaternion(Body);
3213 return; 2634 d.Vector3 vel = d.BodyGetLinearVel(Body);
3214 2635 d.Vector3 rotvel = d.BodyGetAngularVel(Body);
3215 2636 d.Vector3 torque = d.BodyGetTorque(Body);
3216 d.BodyEnable(Body); 2637 _torque = new Vector3(torque.X, torque.Y, torque.Z);
3217 } 2638 Vector3 l_position = Vector3.Zero;
3218 2639 Quaternion l_orientation = Quaternion.Identity;
3219 bodydisablecontrol = 0; 2640
3220 2641 // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!)
3221 d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator 2642 //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
3222 2643 //if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
3223 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) 2644 //if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
3224 { 2645 //if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
3225 // 'VEHICLES' are dealt with in ODEDynamics.cs 2646
3226 m_vehicle.Step(); 2647 m_lastposition = _position;
3227 return; 2648 m_lastorientation = _orientation;
3228 } 2649
3229 2650 l_position.X = vec.X;
3230 float fx = 0; 2651 l_position.Y = vec.Y;
3231 float fy = 0; 2652 l_position.Z = vec.Z;
3232 float fz = 0; 2653 l_orientation.X = ori.X;
3233 2654 l_orientation.Y = ori.Y;
3234 float m_mass = _mass; 2655 l_orientation.Z = ori.Z;
3235 2656 l_orientation.W = ori.W;
3236 if (m_usePID && m_PIDTau > 0) 2657
3237 { 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)
3238 // for now position error
3239 _target_velocity =
3240 new Vector3(
3241 (m_PIDTarget.X - lpos.X),
3242 (m_PIDTarget.Y - lpos.Y),
3243 (m_PIDTarget.Z - lpos.Z)
3244 );
3245
3246 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.02f))
3247 {
3248 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3249 d.BodySetLinearVel(Body, 0, 0, 0);
3250 return;
3251 }
3252 else
3253 {
3254 _zeroFlag = false;
3255
3256 float tmp = 1 / m_PIDTau;
3257 _target_velocity *= tmp;
3258
3259 // apply limits
3260 tmp = _target_velocity.Length();
3261 if (tmp > 50.0f)
3262 {
3263 tmp = 50 / tmp;
3264 _target_velocity *= tmp;
3265 }
3266 else if (tmp < 0.05f)
3267 {
3268 tmp = 0.05f / tmp;
3269 _target_velocity *= tmp;
3270 }
3271
3272 d.Vector3 vel = d.BodyGetLinearVel(Body);
3273 fx = (_target_velocity.X - vel.X) * m_invTimeStep;
3274 fy = (_target_velocity.Y - vel.Y) * m_invTimeStep;
3275 fz = (_target_velocity.Z - vel.Z) * m_invTimeStep;
3276// d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
3277 }
3278 } // end if (m_usePID)
3279
3280 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
3281 else if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0)
3282 {
3283
3284 // Non-Vehicles have a limited set of Hover options.
3285 // determine what our target height really is based on HoverType
3286
3287 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(lpos.X, lpos.Y);
3288
3289 switch (m_PIDHoverType)
3290 {
3291 case PIDHoverType.Ground:
3292 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3293 break;
3294
3295 case PIDHoverType.GroundAndWater:
3296 m_waterHeight = _parent_scene.GetWaterLevel();
3297 if (m_groundHeight > m_waterHeight)
3298 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3299 else
3300 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
3301 break;
3302 } // end switch (m_PIDHoverType)
3303
3304 // don't go underground unless volumedetector
3305
3306 if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect)
3307 { 2659 {
3308 d.Vector3 vel = d.BodyGetLinearVel(Body); 2660 //base.RaiseOutOfBounds(l_position);
3309 2661
3310 fz = (m_targetHoverHeight - lpos.Z); 2662 if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
3311
3312 // if error is zero, use position control; otherwise, velocity control
3313 if (Math.Abs(fz) < 0.01f)
3314 { 2663 {
3315 d.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight); 2664 _position = l_position;
3316 d.BodySetLinearVel(Body, vel.X, vel.Y, 0); 2665 //_parent_scene.remActivePrim(this);
2666 if (_parent == null)
2667 base.RequestPhysicsterseUpdate();
2668 return;
3317 } 2669 }
3318 else 2670 else
3319 { 2671 {
3320 _zeroFlag = false; 2672 if (_parent == null)
3321 fz /= m_PIDHoverTau; 2673 base.RaiseOutOfBounds(l_position);
3322 2674 return;
3323 float tmp = Math.Abs(fz);
3324 if (tmp > 50)
3325 fz = 50 * Math.Sign(fz);
3326 else if (tmp < 0.1)
3327 fz = 0.1f * Math.Sign(fz);
3328
3329 fz = ((fz - vel.Z) * m_invTimeStep);
3330 } 2675 }
3331 } 2676 }
3332 }
3333 else
3334 {
3335 float b = (1.0f - m_buoyancy);
3336 fx = _parent_scene.gravityx * b;
3337 fy = _parent_scene.gravityy * b;
3338 fz = _parent_scene.gravityz * b;
3339 }
3340
3341 fx *= m_mass;
3342 fy *= m_mass;
3343 fz *= m_mass;
3344
3345 // constant force
3346 fx += m_force.X;
3347 fy += m_force.Y;
3348 fz += m_force.Z;
3349
3350 fx += m_forceacc.X;
3351 fy += m_forceacc.Y;
3352 fz += m_forceacc.Z;
3353
3354 m_forceacc = Vector3.Zero;
3355
3356 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
3357 if (fx != 0 || fy != 0 || fz != 0)
3358 {
3359 d.BodyAddForce(Body, fx, fy, fz);
3360 //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
3361 }
3362 2677
3363 Vector3 trq; 2678 if (l_position.Z < 0)
3364
3365 trq = _torque;
3366 trq += m_angularForceacc;
3367 m_angularForceacc = Vector3.Zero;
3368 if (trq.X != 0 || trq.Y != 0 || trq.Z != 0)
3369 {
3370 d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z);
3371 }
3372 }
3373 else
3374 { // is not physical, or is not a body or is selected
3375 // _zeroPosition = d.BodyGetPosition(Body);
3376 return;
3377 //Console.WriteLine("Nothing " + Name);
3378
3379 }
3380 }
3381
3382 public void UpdatePositionAndVelocity()
3383 {
3384 if (_parent == null && !m_disabled && !m_building && !m_outbounds && Body != IntPtr.Zero)
3385 {
3386 if (d.BodyIsEnabled(Body) || !_zeroFlag)
3387 {
3388 bool lastZeroFlag = _zeroFlag;
3389
3390 d.Vector3 lpos = d.GeomGetPosition(prim_geom);
3391
3392 // check outside region
3393 if (lpos.Z < -100 || lpos.Z > 100000f)
3394 { 2679 {
3395 m_outbounds = true; 2680 // This is so prim that get lost underground don't fall forever and suck up
2681 //
2682 // Sim resources and memory.
2683 // Disables the prim's movement physics....
2684 // It's a hack and will generate a console message if it fails.
2685
2686 //IsPhysical = false;
2687 if (_parent == null)
2688 base.RaiseOutOfBounds(_position);
3396 2689
3397 lpos.Z = Util.Clip(lpos.Z, -100f, 100000f);
3398 _acceleration.X = 0; 2690 _acceleration.X = 0;
3399 _acceleration.Y = 0; 2691 _acceleration.Y = 0;
3400 _acceleration.Z = 0; 2692 _acceleration.Z = 0;
@@ -3406,437 +2698,589 @@ namespace OpenSim.Region.Physics.OdePlugin
3406 m_rotationalVelocity.Y = 0; 2698 m_rotationalVelocity.Y = 0;
3407 m_rotationalVelocity.Z = 0; 2699 m_rotationalVelocity.Z = 0;
3408 2700
3409 d.BodySetLinearVel(Body, 0, 0, 0); // stop it 2701 if (_parent == null)
3410 d.BodySetAngularVel(Body, 0, 0, 0); // stop it 2702 base.RequestPhysicsterseUpdate();
3411 d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere
3412 m_lastposition = _position;
3413 m_lastorientation = _orientation;
3414
3415 base.RequestPhysicsterseUpdate();
3416 2703
3417// throttleCounter = 0; 2704 m_throttleUpdates = false;
2705 throttleCounter = 0;
3418 _zeroFlag = true; 2706 _zeroFlag = true;
3419 2707 //outofBounds = true;
3420 disableBodySoft(); // disable it and colisions
3421 base.RaiseOutOfBounds(_position);
3422 return;
3423 } 2708 }
3424 2709
3425 if (lpos.X < 0f) 2710 //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation));
3426 { 2711//Console.WriteLine("Adiff " + Name + " = " + Adiff);
3427 _position.X = Util.Clip(lpos.X, -2f, -0.1f); 2712 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
3428 m_outbounds = true; 2713 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
3429 } 2714 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
3430 else if (lpos.X > _parent_scene.WorldExtents.X) 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
3431 { 2717 {
3432 _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); 2718 _zeroFlag = true;
3433 m_outbounds = true; 2719//Console.WriteLine("ZFT 2");
3434 } 2720 m_throttleUpdates = false;
3435 if (lpos.Y < 0f)
3436 {
3437 _position.Y = Util.Clip(lpos.Y, -2f, -0.1f);
3438 m_outbounds = true;
3439 } 2721 }
3440 else if (lpos.Y > _parent_scene.WorldExtents.Y) 2722 else
3441 { 2723 {
3442 _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); 2724 //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
3443 m_outbounds = true; 2725 _zeroFlag = false;
2726 m_lastUpdateSent = false;
2727 //m_throttleUpdates = false;
3444 } 2728 }
3445 2729
3446 if (m_outbounds) 2730 if (_zeroFlag)
3447 { 2731 {
3448 m_lastposition = _position; 2732 _velocity.X = 0.0f;
3449 m_lastorientation = _orientation; 2733 _velocity.Y = 0.0f;
3450 2734 _velocity.Z = 0.0f;
3451 d.Vector3 dtmp = d.BodyGetAngularVel(Body);
3452 m_rotationalVelocity.X = dtmp.X;
3453 m_rotationalVelocity.Y = dtmp.Y;
3454 m_rotationalVelocity.Z = dtmp.Z;
3455
3456 dtmp = d.BodyGetLinearVel(Body);
3457 _velocity.X = dtmp.X;
3458 _velocity.Y = dtmp.Y;
3459 _velocity.Z = dtmp.Z;
3460
3461 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
3462 d.BodySetAngularVel(Body, 0, 0, 0);
3463 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
3464 disableBodySoft(); // stop collisions
3465 UnSubscribeEvents();
3466
3467 base.RequestPhysicsterseUpdate();
3468 return;
3469 }
3470 2735
3471 d.Quaternion ori; 2736 _acceleration.X = 0;
3472 d.GeomCopyQuaternion(prim_geom, out ori); 2737 _acceleration.Y = 0;
3473 2738 _acceleration.Z = 0;
3474 // decide if moving 2739
3475 // use positions since this are integrated quantities 2740 //_orientation.w = 0f;
3476 // tolerance values depende a lot on simulation noise... 2741 //_orientation.X = 0f;
3477 // use simple math.abs since we dont need to be exact 2742 //_orientation.Y = 0f;
3478 2743 //_orientation.Z = 0f;
3479 if ( 2744 m_rotationalVelocity.X = 0;
3480 (Math.Abs(_position.X - lpos.X) < 0.001f) 2745 m_rotationalVelocity.Y = 0;
3481 && (Math.Abs(_position.Y - lpos.Y) < 0.001f) 2746 m_rotationalVelocity.Z = 0;
3482 && (Math.Abs(_position.Z - lpos.Z) < 0.001f) 2747 if (!m_lastUpdateSent)
3483 && (Math.Abs(_orientation.X - ori.X) < 0.0001f) 2748 {
3484 && (Math.Abs(_orientation.Y - ori.Y) < 0.0001f) 2749 m_throttleUpdates = false;
3485 && (Math.Abs(_orientation.Z - ori.Z) < 0.0001f) // ignore W 2750 throttleCounter = 0;
3486 ) 2751 m_rotationalVelocity = pv;
3487 { 2752
3488 _zeroFlag = true; 2753 if (_parent == null)
2754 {
2755 base.RequestPhysicsterseUpdate();
2756 }
2757
2758 m_lastUpdateSent = true;
2759 }
3489 } 2760 }
3490 else 2761 else
3491 _zeroFlag = false;
3492
3493 // update velocities and aceleration
3494 if (!(_zeroFlag && lastZeroFlag))
3495 { 2762 {
3496 d.Vector3 vel = d.BodyGetLinearVel(Body); 2763 if (lastZeroFlag != _zeroFlag)
2764 {
2765 if (_parent == null)
2766 {
2767 base.RequestPhysicsterseUpdate();
2768 }
2769 }
2770
2771 m_lastVelocity = _velocity;
3497 2772
3498 _acceleration = _velocity; 2773 _position = l_position;
3499 2774
3500 if ((Math.Abs(vel.X) < 0.001f) && 2775 _velocity.X = vel.X;
3501 (Math.Abs(vel.Y) < 0.001f) && 2776 _velocity.Y = vel.Y;
3502 (Math.Abs(vel.Z) < 0.001f)) 2777 _velocity.Z = vel.Z;
2778
2779 _acceleration = ((_velocity - m_lastVelocity) / 0.1f);
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);
2781 //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());
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))
3503 { 2788 {
3504 _velocity = Vector3.Zero; 2789 m_minvelocity = 0.5f;
3505 float t = -m_invTimeStep;
3506 _acceleration = _acceleration * t;
3507 } 2790 }
3508 else 2791 else
3509 { 2792 {
3510 _velocity.X = vel.X; 2793 m_minvelocity = 0.02f;
3511 _velocity.Y = vel.Y;
3512 _velocity.Z = vel.Z;
3513 _acceleration = (_velocity - _acceleration) * m_invTimeStep;
3514 }
3515
3516 if ((Math.Abs(_acceleration.X) < 0.01f) &&
3517 (Math.Abs(_acceleration.Y) < 0.01f) &&
3518 (Math.Abs(_acceleration.Z) < 0.01f))
3519 {
3520 _acceleration = Vector3.Zero;
3521 } 2794 }
3522 2795
3523 if ((Math.Abs(_orientation.X - ori.X) < 0.0001) && 2796 if (_velocity.ApproxEquals(pv, m_minvelocity))
3524 (Math.Abs(_orientation.Y - ori.Y) < 0.0001) &&
3525 (Math.Abs(_orientation.Z - ori.Z) < 0.0001)
3526 )
3527 { 2797 {
3528 m_rotationalVelocity = Vector3.Zero; 2798 m_rotationalVelocity = pv;
3529 } 2799 }
3530 else 2800 else
3531 { 2801 {
3532 vel = d.BodyGetAngularVel(Body); 2802 m_rotationalVelocity = new Vector3(rotvel.X, rotvel.Y, rotvel.Z);
3533 m_rotationalVelocity.X = vel.X;
3534 m_rotationalVelocity.Y = vel.Y;
3535 m_rotationalVelocity.Z = vel.Z;
3536 } 2803 }
3537 }
3538 2804
3539 if (_zeroFlag) 2805 //m_log.Debug("ODE: " + m_rotationalVelocity.ToString());
3540 { 2806 _orientation.X = ori.X;
3541 if (lastZeroFlag) 2807 _orientation.Y = ori.Y;
2808 _orientation.Z = ori.Z;
2809 _orientation.W = ori.W;
2810 m_lastUpdateSent = false;
2811 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
3542 { 2812 {
3543 _velocity = Vector3.Zero; 2813 if (_parent == null)
3544 _acceleration = Vector3.Zero; 2814 {
3545 m_rotationalVelocity = Vector3.Zero; 2815 base.RequestPhysicsterseUpdate();
2816 }
3546 } 2817 }
3547 2818 else
3548 if (!m_lastUpdateSent)
3549 { 2819 {
3550 base.RequestPhysicsterseUpdate(); 2820 throttleCounter++;
3551 if (lastZeroFlag)
3552 m_lastUpdateSent = true;
3553 } 2821 }
3554 return;
3555 } 2822 }
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;
3556 2831
3557 _position.X = lpos.X; 2832 _acceleration.X = 0;
3558 _position.Y = lpos.Y; 2833 _acceleration.Y = 0;
3559 _position.Z = lpos.Z; 2834 _acceleration.Z = 0;
3560 2835
3561 _orientation.X = ori.X; 2836 m_rotationalVelocity.X = 0;
3562 _orientation.Y = ori.Y; 2837 m_rotationalVelocity.Y = 0;
3563 _orientation.Z = ori.Z; 2838 m_rotationalVelocity.Z = 0;
3564 _orientation.W = ori.W; 2839 _zeroFlag = true;
3565 base.RequestPhysicsterseUpdate();
3566 m_lastUpdateSent = false;
3567 } 2840 }
3568 } 2841 }
3569 } 2842 }
3570 2843
3571 internal static bool QuaternionIsFinite(Quaternion q) 2844 public override bool FloatOnWater
3572 { 2845 {
3573 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) 2846 set {
3574 return false; 2847 m_taintCollidesWater = value;
3575 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) 2848 _parent_scene.AddPhysicsActorTaint(this);
3576 return false; 2849 }
3577 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
3578 return false;
3579 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
3580 return false;
3581 return true;
3582 } 2850 }
3583 2851
3584 internal static void DMassSubPartFromObj(ref d.Mass part, ref d.Mass theobj) 2852 public override void SetMomentum(Vector3 momentum)
3585 { 2853 {
3586 // assumes object center of mass is zero 2854 }
3587 float smass = part.mass; 2855
3588 theobj.mass -= smass; 2856 public override Vector3 PIDTarget
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; } }
3589 2877
3590 smass *= 1.0f / (theobj.mass); ; 2878 public override bool APIDActive{ set { return; } }
3591 2879
3592 theobj.c.X -= part.c.X * smass; 2880 public override float APIDStrength{ set { return; } }
3593 theobj.c.Y -= part.c.Y * smass;
3594 theobj.c.Z -= part.c.Z * smass;
3595 2881
3596 theobj.I.M00 -= part.I.M00; 2882 public override float APIDDamping{ set { return; } }
3597 theobj.I.M01 -= part.I.M01;
3598 theobj.I.M02 -= part.I.M02;
3599 theobj.I.M10 -= part.I.M10;
3600 theobj.I.M11 -= part.I.M11;
3601 theobj.I.M12 -= part.I.M12;
3602 theobj.I.M20 -= part.I.M20;
3603 theobj.I.M21 -= part.I.M21;
3604 theobj.I.M22 -= part.I.M22;
3605 }
3606 2883
3607 private void donullchange() 2884 private void createAMotor(Vector3 axis)
3608 { 2885 {
3609 } 2886 if (Body == IntPtr.Zero)
2887 return;
3610 2888
3611 public bool DoAChange(changes what, object arg) 2889 if (Amotor != IntPtr.Zero)
3612 {
3613 if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.AddPhysRep && what != changes.Remove)
3614 { 2890 {
3615 return false; 2891 d.JointDestroy(Amotor);
2892 Amotor = IntPtr.Zero;
3616 } 2893 }
3617 2894
3618 // nasty switch 2895 float axisnum = 3;
3619 switch (what)
3620 {
3621 case changes.Add:
3622 changeadd();
3623 break;
3624
3625 case changes.AddPhysRep:
3626 changeAddPhysRep((ODEPhysRepData)arg);
3627 break;
3628
3629 case changes.Remove:
3630 //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff...
3631 //When we return true, it destroys all of the prims in the linkset anyway
3632 if (_parent != null)
3633 {
3634 OdePrim parent = (OdePrim)_parent;
3635 parent.ChildRemove(this, false);
3636 }
3637 else
3638 ChildRemove(this, false);
3639 2896
3640 m_vehicle = null; 2897 axisnum = (axisnum - (axis.X + axis.Y + axis.Z));
3641 RemoveGeom();
3642 m_targetSpace = IntPtr.Zero;
3643 UnSubscribeEvents();
3644 return true;
3645 2898
3646 case changes.Link: 2899 // PhysicsVector totalSize = new PhysicsVector(_size.X, _size.Y, _size.Z);
3647 OdePrim tmp = (OdePrim)arg;
3648 changeLink(tmp);
3649 break;
3650 2900
3651 case changes.DeLink: 2901
3652 changeLink(null); 2902 // Inverse Inertia Matrix, set the X, Y, and/r Z inertia to 0 then invert it again.
3653 break; 2903 d.Mass objMass;
2904 d.MassSetZero(out objMass);
2905 DMassCopy(ref pMass, ref objMass);
3654 2906
3655 case changes.Position: 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);
3656 changePosition((Vector3)arg);
3657 break;
3658 2908
3659 case changes.Orientation: 2909 Matrix4 dMassMat = FromDMass(objMass);
3660 changeOrientation((Quaternion)arg);
3661 break;
3662 2910
3663 case changes.PosOffset: 2911 Matrix4 mathmat = Inverse(dMassMat);
3664 donullchange();
3665 break;
3666 2912
3667 case changes.OriOffset: 2913 /*
3668 donullchange(); 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]);
3669 break;
3670 2915
3671 case changes.Velocity: 2916 mathmat = Inverse(mathmat);
3672 changevelocity((Vector3)arg);
3673 break;
3674 2917
3675// case changes.Acceleration:
3676// changeacceleration((Vector3)arg);
3677// break;
3678 2918
3679 case changes.AngVelocity: 2919 objMass = FromMatrix4(mathmat, ref objMass);
3680 changeangvelocity((Vector3)arg); 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);
3681 break;
3682 2921
3683 case changes.Force: 2922 mathmat = Inverse(mathmat);
3684 changeForce((Vector3)arg); 2923 */
3685 break; 2924 if (axis.X == 0)
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
3686 2941
3687 case changes.Torque: 2942 mathmat = Inverse(mathmat);
3688 changeSetTorque((Vector3)arg); 2943 objMass = FromMatrix4(mathmat, ref objMass);
3689 break; 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);
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 }
3690 2955
3691 case changes.AddForce: 2956 if (axisnum <= 0)
3692 changeAddForce((Vector3)arg); 2957 return;
3693 break; 2958 // int dAMotorEuler = 1;
3694 2959
3695 case changes.AddAngForce: 2960 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
3696 changeAddAngularImpulse((Vector3)arg); 2961 d.JointAttach(Amotor, Body, IntPtr.Zero);
3697 break; 2962 d.JointSetAMotorMode(Amotor, 0);
3698 2963
3699 case changes.AngLock: 2964 d.JointSetAMotorNumAxes(Amotor,(int)axisnum);
3700 changeAngularLock((Vector3)arg); 2965 int i = 0;
3701 break;
3702 2966
3703 case changes.Size: 2967 if (axis.X == 0)
3704 changeSize((Vector3)arg); 2968 {
3705 break; 2969 d.JointSetAMotorAxis(Amotor, i, 0, 1, 0, 0);
2970 i++;
2971 }
3706 2972
3707 case changes.Shape: 2973 if (axis.Y == 0)
3708 changeShape((PrimitiveBaseShape)arg); 2974 {
3709 break; 2975 d.JointSetAMotorAxis(Amotor, i, 0, 0, 1, 0);
2976 i++;
2977 }
3710 2978
3711 case changes.PhysRepData: 2979 if (axis.Z == 0)
3712 changePhysRepData((ODEPhysRepData) arg); 2980 {
3713 break; 2981 d.JointSetAMotorAxis(Amotor, i, 0, 0, 0, 1);
2982 i++;
2983 }
3714 2984
3715 case changes.CollidesWater: 2985 for (int j = 0; j < (int)axisnum; j++)
3716 changeFloatOnWater((bool)arg); 2986 {
3717 break; 2987 //d.JointSetAMotorAngle(Amotor, j, 0);
2988 }
3718 2989
3719 case changes.VolumeDtc: 2990 //d.JointSetAMotorAngle(Amotor, 1, 0);
3720 changeVolumedetetion((bool)arg); 2991 //d.JointSetAMotorAngle(Amotor, 2, 0);
3721 break;
3722 2992
3723 case changes.Phantom: 2993 // These lowstops and high stops are effectively (no wiggle room)
3724 changePhantomStatus((bool)arg); 2994 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0f);
3725 break; 2995 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f);
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 }
3726 3004
3727 case changes.Physical: 3005 private Matrix4 FromDMass(d.Mass pMass)
3728 changePhysicsStatus((bool)arg); 3006 {
3729 break; 3007 Matrix4 obj;
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 }
3730 3026
3731 case changes.Selected: 3027 private d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
3732 changeSelectedStatus((bool)arg); 3028 {
3733 break; 3029 obj.I.M00 = pMat[0, 0];
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 }
3734 3040
3735 case changes.disabled: 3041 public override void SubscribeEvents(int ms)
3736 changeDisable((bool)arg); 3042 {
3737 break; 3043 m_eventsubscription = ms;
3044 _parent_scene.AddCollisionEventReporting(this);
3045 }
3738 3046
3739 case changes.building: 3047 public override void UnSubscribeEvents()
3740 changeBuilding((bool)arg); 3048 {
3741 break; 3049 _parent_scene.RemoveCollisionEventReporting(this);
3050 m_eventsubscription = 0;
3051 }
3742 3052
3743 case changes.VehicleType: 3053 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
3744 changeVehicleType((int)arg); 3054 {
3745 break; 3055 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
3056 }
3746 3057
3747 case changes.VehicleFlags: 3058 public void SendCollisions()
3748 changeVehicleFlags((strVehicleBoolParam) arg); 3059 {
3749 break; 3060 if (m_collisionsOnPreviousFrame || CollisionEventsThisFrame.Count > 0)
3061 {
3062 base.SendCollisionUpdate(CollisionEventsThisFrame);
3750 3063
3751 case changes.VehicleFloatParam: 3064 if (CollisionEventsThisFrame.Count > 0)
3752 changeVehicleFloatParam((strVehicleFloatParam) arg); 3065 {
3753 break; 3066 m_collisionsOnPreviousFrame = true;
3067 CollisionEventsThisFrame.Clear();
3068 }
3069 else
3070 {
3071 m_collisionsOnPreviousFrame = false;
3072 }
3073 }
3074 }
3754 3075
3755 case changes.VehicleVectorParam: 3076 public override bool SubscribedEvents()
3756 changeVehicleVectorParam((strVehicleVectorParam) arg); 3077 {
3757 break; 3078 if (m_eventsubscription > 0)
3079 return true;
3080 return false;
3081 }
3758 3082
3759 case changes.VehicleRotationParam: 3083 public static Matrix4 Inverse(Matrix4 pMat)
3760 changeVehicleRotationParam((strVehicleQuatParam) arg); 3084 {
3761 break; 3085 if (determinant3x3(pMat) == 0)
3086 {
3087 return Matrix4.Identity; // should probably throw an error. singluar matrix inverse not possible
3088 }
3762 3089
3763 case changes.SetVehicle: 3090 return (Adjoint(pMat) / determinant3x3(pMat));
3764 changeSetVehicle((VehicleData) arg); 3091 }
3765 break;
3766 3092
3767 case changes.Buoyancy: 3093 public static Matrix4 Adjoint(Matrix4 pMat)
3768 changeBuoyancy((float)arg); 3094 {
3769 break; 3095 Matrix4 adjointMatrix = new Matrix4();
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 }
3770 3103
3771 case changes.PIDTarget: 3104 adjointMatrix = Transpose(adjointMatrix);
3772 changePIDTarget((Vector3)arg); 3105 return adjointMatrix;
3773 break; 3106 }
3774 3107
3775 case changes.PIDTau: 3108 public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol)
3776 changePIDTau((float)arg); 3109 {
3777 break; 3110 Matrix4 minor = new Matrix4();
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 }
3778 3126
3779 case changes.PIDActive: 3127 return minor;
3780 changePIDActive((bool)arg); 3128 }
3781 break;
3782 3129
3783 case changes.PIDHoverHeight: 3130 public static Matrix4 Transpose(Matrix4 pMat)
3784 changePIDHoverHeight((float)arg); 3131 {
3785 break; 3132 Matrix4 transposeMatrix = new Matrix4();
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 }
3786 3138
3787 case changes.PIDHoverType: 3139 public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val)
3788 changePIDHoverType((PIDHoverType)arg); 3140 {
3789 break; 3141 switch (r)
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 }
3790 3159
3791 case changes.PIDHoverTau:
3792 changePIDHoverTau((float)arg);
3793 break; 3160 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 }
3794 3177
3795 case changes.PIDHoverActive:
3796 changePIDHoverActive((bool)arg);
3797 break; 3178 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 }
3798 3195
3799 case changes.Null:
3800 donullchange();
3801 break; 3196 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 }
3802 3213
3803
3804
3805 default:
3806 donullchange();
3807 break; 3214 break;
3808 } 3215 }
3809 return false;
3810 } 3216 }
3811 3217
3812 public void AddChange(changes what, object arg) 3218 private static float determinant3x3(Matrix4 pMat)
3813 { 3219 {
3814 _parent_scene.AddChange((PhysicsActor) this, what, arg); 3220 float det = 0;
3815 } 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];
3816 3227
3817 3228 det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6);
3818 private struct strVehicleBoolParam 3229 return det;
3819 { 3230 }
3820 public int param; 3231
3821 public bool value; 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;
3822 } 3248 }
3823 3249
3824 private struct strVehicleFloatParam 3250 public override void SetMaterial(int pMaterial)
3825 { 3251 {
3826 public int param; 3252 m_material = pMaterial;
3827 public float value;
3828 } 3253 }
3829 3254
3830 private struct strVehicleQuatParam 3255 private void CheckMeshAsset()
3831 { 3256 {
3832 public int param; 3257 if (_pbs.SculptEntry && !m_assetFailed && _pbs.SculptTexture != UUID.Zero)
3833 public Quaternion value; 3258 {
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 }
3834 } 3267 }
3835 3268
3836 private struct strVehicleVectorParam 3269 void MeshAssetReveived(AssetBase asset)
3837 { 3270 {
3838 public int param; 3271 if (asset.Data != null && asset.Data.Length > 0)
3839 public Vector3 value; 3272 {
3840 } 3273 if (!_pbs.SculptEntry)
3274 return;
3275 if (_pbs.SculptTexture.ToString() != asset.ID)
3276 return;
3277
3278 _pbs.SculptData = new byte[asset.Data.Length];
3279 asset.Data.CopyTo(_pbs.SculptData, 0);
3280 m_assetFailed = false;
3281 m_taintshape = true;
3282 _parent_scene.AddPhysicsActorTaint(this);
3283 }
3284 }
3841 } 3285 }
3842} 3286} \ No newline at end of file
generated by cgit v1.1 at 2024-12-18 05:46:54 +0000