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