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