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.cs3387
1 files changed, 3387 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..f934b8a
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs
@@ -0,0 +1,3387 @@
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.Physics.Manager;
54
55namespace OpenSim.Region.Physics.OdePlugin
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.Zero, 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, prim.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 // The body doesn't already have a finite rotation mode set here
1188 if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
1189 {
1190 prm.createAMotor(m_angularlock);
1191 }
1192 prm.Body = Body;
1193 _parent_scene.ActivatePrim(prm);
1194 }
1195
1196 m_collisionCategories |= CollisionCategories.Body;
1197 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1198
1199 if (m_assetFailed)
1200 {
1201 d.GeomSetCategoryBits(prim_geom, 0);
1202 d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
1203 }
1204 else
1205 {
1206 //Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + Name);
1207 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1208 //Console.WriteLine(" Post GeomSetCategoryBits 2");
1209 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1210 }
1211
1212 d.Quaternion quat2 = new d.Quaternion();
1213 quat2.W = _orientation.W;
1214 quat2.X = _orientation.X;
1215 quat2.Y = _orientation.Y;
1216 quat2.Z = _orientation.Z;
1217
1218 d.Matrix3 mat2 = new d.Matrix3();
1219 d.RfromQ(out mat2, ref quat2);
1220 d.GeomSetBody(prim_geom, Body);
1221 d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z);
1222 //d.GeomSetOffsetPosition(prim.prim_geom,
1223 // (Position.X - prm.Position.X) - pMass.c.X,
1224 // (Position.Y - prm.Position.Y) - pMass.c.Y,
1225 // (Position.Z - prm.Position.Z) - pMass.c.Z);
1226 //d.GeomSetOffsetRotation(prim_geom, ref mat2);
1227 d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
1228 d.BodySetMass(Body, ref pMass);
1229
1230 d.BodySetAutoDisableFlag(Body, true);
1231 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
1232
1233 m_interpenetrationcount = 0;
1234 m_collisionscore = 0;
1235 m_disabled = false;
1236
1237 // The body doesn't already have a finite rotation mode set here
1238 if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
1239 {
1240 createAMotor(m_angularlock);
1241 }
1242
1243 d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
1244
1245 if (m_vehicle.Type != Vehicle.TYPE_NONE)
1246 m_vehicle.Enable(Body, _parent_scene);
1247
1248 _parent_scene.ActivatePrim(this);
1249 }
1250 }
1251
1252 private void ChildSetGeom(OdePrim odePrim)
1253 {
1254// m_log.DebugFormat(
1255// "[ODE PRIM]: ChildSetGeom {0} {1} for {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID);
1256
1257 //if (IsPhysical && Body != IntPtr.Zero)
1258 lock (childrenPrim)
1259 {
1260 foreach (OdePrim prm in childrenPrim)
1261 {
1262 //prm.childPrim = true;
1263 prm.disableBody();
1264 //prm.m_taintparent = null;
1265 //prm._parent = null;
1266 //prm.m_taintPhysics = false;
1267 //prm.m_disabled = true;
1268 //prm.childPrim = false;
1269 }
1270 }
1271
1272 disableBody();
1273
1274 // Spurious - Body == IntPtr.Zero after disableBody()
1275// if (Body != IntPtr.Zero)
1276// {
1277// _parent_scene.DeactivatePrim(this);
1278// }
1279
1280 lock (childrenPrim)
1281 {
1282 foreach (OdePrim prm in childrenPrim)
1283 {
1284//Console.WriteLine("ChildSetGeom calls ParentPrim");
1285 AddChildPrim(prm);
1286 }
1287 }
1288 }
1289
1290 private void ChildDelink(OdePrim odePrim)
1291 {
1292// m_log.DebugFormat(
1293// "[ODE PRIM]: Delinking prim {0} {1} from {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID);
1294
1295 // Okay, we have a delinked child.. need to rebuild the body.
1296 lock (childrenPrim)
1297 {
1298 foreach (OdePrim prm in childrenPrim)
1299 {
1300 prm.childPrim = true;
1301 prm.disableBody();
1302 //prm.m_taintparent = null;
1303 //prm._parent = null;
1304 //prm.m_taintPhysics = false;
1305 //prm.m_disabled = true;
1306 //prm.childPrim = false;
1307 }
1308 }
1309
1310 disableBody();
1311
1312 lock (childrenPrim)
1313 {
1314 //Console.WriteLine("childrenPrim.Remove " + odePrim);
1315 childrenPrim.Remove(odePrim);
1316 }
1317
1318 // Spurious - Body == IntPtr.Zero after disableBody()
1319// if (Body != IntPtr.Zero)
1320// {
1321// _parent_scene.DeactivatePrim(this);
1322// }
1323
1324 lock (childrenPrim)
1325 {
1326 foreach (OdePrim prm in childrenPrim)
1327 {
1328//Console.WriteLine("ChildDelink calls ParentPrim");
1329 AddChildPrim(prm);
1330 }
1331 }
1332 }
1333
1334 /// <summary>
1335 /// Change prim in response to a selection taint.
1336 /// </summary>
1337 private void changeSelectedStatus()
1338 {
1339 if (m_taintselected)
1340 {
1341 m_collisionCategories = CollisionCategories.Selected;
1342 m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
1343
1344 // We do the body disable soft twice because 'in theory' a collision could have happened
1345 // in between the disabling and the collision properties setting
1346 // which would wake the physical body up from a soft disabling and potentially cause it to fall
1347 // through the ground.
1348
1349 // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select
1350 // just one part of the assembly, the rest of the assembly is non-selected and still simulating,
1351 // so that causes the selected part to wake up and continue moving.
1352
1353 // even if you select all parts of a jointed assembly, it is not guaranteed that the entire
1354 // assembly will stop simulating during the selection, because of the lack of atomicity
1355 // of select operations (their processing could be interrupted by a thread switch, causing
1356 // simulation to continue before all of the selected object notifications trickle down to
1357 // the physics engine).
1358
1359 // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are
1360 // selected and disabled. then, due to a thread switch, the selection processing is
1361 // interrupted and the physics engine continues to simulate, so the last 50 items, whose
1362 // selection was not yet processed, continues to simulate. this wakes up ALL of the
1363 // first 50 again. then the last 50 are disabled. then the first 50, which were just woken
1364 // up, start simulating again, which in turn wakes up the last 50.
1365
1366 if (IsPhysical)
1367 {
1368 disableBodySoft();
1369 }
1370
1371 if (m_assetFailed)
1372 {
1373 d.GeomSetCategoryBits(prim_geom, 0);
1374 d.GeomSetCollideBits(prim_geom, 0);
1375 }
1376 else
1377 {
1378 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1379 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1380 }
1381
1382 if (IsPhysical)
1383 {
1384 disableBodySoft();
1385 }
1386 }
1387 else
1388 {
1389 m_collisionCategories = CollisionCategories.Geom;
1390
1391 if (IsPhysical)
1392 m_collisionCategories |= CollisionCategories.Body;
1393
1394 m_collisionFlags = m_default_collisionFlags;
1395
1396 if (m_collidesLand)
1397 m_collisionFlags |= CollisionCategories.Land;
1398 if (m_collidesWater)
1399 m_collisionFlags |= CollisionCategories.Water;
1400
1401 if (m_assetFailed)
1402 {
1403 d.GeomSetCategoryBits(prim_geom, 0);
1404 d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
1405 }
1406 else
1407 {
1408 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1409 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1410 }
1411
1412 if (IsPhysical)
1413 {
1414 if (Body != IntPtr.Zero)
1415 {
1416 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1417 d.BodySetForce(Body, 0, 0, 0);
1418 enableBodySoft();
1419 }
1420 }
1421 }
1422
1423 resetCollisionAccounting();
1424 m_isSelected = m_taintselected;
1425 }//end changeSelectedStatus
1426
1427 internal void ResetTaints()
1428 {
1429 m_taintposition = _position;
1430 m_taintrot = _orientation;
1431 m_taintPhysics = IsPhysical;
1432 m_taintselected = m_isSelected;
1433 m_taintsize = _size;
1434 m_taintshape = false;
1435 m_taintforce = false;
1436 m_taintdisable = false;
1437 m_taintVelocity = Vector3.Zero;
1438 }
1439
1440 /// <summary>
1441 /// Create a geometry for the given mesh in the given target space.
1442 /// </summary>
1443 /// <param name="m_targetSpace"></param>
1444 /// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param>
1445 private void CreateGeom(IntPtr m_targetSpace, IMesh mesh)
1446 {
1447#if SPAM
1448Console.WriteLine("CreateGeom:");
1449#endif
1450 if (mesh != null)
1451 {
1452 setMesh(_parent_scene, mesh);
1453 }
1454 else
1455 {
1456 if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1)
1457 {
1458 if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z)
1459 {
1460 if (((_size.X / 2f) > 0f))
1461 {
1462// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1463 try
1464 {
1465//Console.WriteLine(" CreateGeom 1");
1466 SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2));
1467 m_expectedCollisionContacts = 3;
1468 }
1469 catch (AccessViolationException)
1470 {
1471 m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
1472 return;
1473 }
1474 }
1475 else
1476 {
1477// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1478 try
1479 {
1480//Console.WriteLine(" CreateGeom 2");
1481 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1482 m_expectedCollisionContacts = 4;
1483 }
1484 catch (AccessViolationException)
1485 {
1486 m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
1487 return;
1488 }
1489 }
1490 }
1491 else
1492 {
1493// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1494 try
1495 {
1496//Console.WriteLine(" CreateGeom 3");
1497 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1498 m_expectedCollisionContacts = 4;
1499 }
1500 catch (AccessViolationException)
1501 {
1502 m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
1503 return;
1504 }
1505 }
1506 }
1507 else
1508 {
1509// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1510 try
1511 {
1512//Console.WriteLine(" CreateGeom 4");
1513 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1514 m_expectedCollisionContacts = 4;
1515 }
1516 catch (AccessViolationException)
1517 {
1518 m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
1519 return;
1520 }
1521 }
1522 }
1523 }
1524
1525 /// <summary>
1526 /// Remove the existing geom from this prim.
1527 /// </summary>
1528 /// <param name="m_targetSpace"></param>
1529 /// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param>
1530 /// <returns>true if the geom was successfully removed, false if it was already gone or the remove failed.</returns>
1531 internal bool RemoveGeom()
1532 {
1533 if (prim_geom != IntPtr.Zero)
1534 {
1535 try
1536 {
1537 _parent_scene.geom_name_map.Remove(prim_geom);
1538 _parent_scene.actor_name_map.Remove(prim_geom);
1539 d.GeomDestroy(prim_geom);
1540 m_expectedCollisionContacts = 0;
1541 prim_geom = IntPtr.Zero;
1542 }
1543 catch (System.AccessViolationException)
1544 {
1545 prim_geom = IntPtr.Zero;
1546 m_expectedCollisionContacts = 0;
1547 m_log.ErrorFormat("[PHYSICS]: PrimGeom dead for {0}", Name);
1548
1549 return false;
1550 }
1551
1552 return true;
1553 }
1554 else
1555 {
1556 m_log.WarnFormat(
1557 "[ODE PRIM]: Called RemoveGeom() on {0} {1} where geometry was already null.", Name, LocalID);
1558
1559 return false;
1560 }
1561 }
1562 /// <summary>
1563 /// Add prim in response to an add taint.
1564 /// </summary>
1565 private void changeadd()
1566 {
1567// m_log.DebugFormat("[ODE PRIM]: Adding prim {0}", Name);
1568
1569 int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
1570 IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
1571
1572 if (targetspace == IntPtr.Zero)
1573 targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
1574
1575 m_targetSpace = targetspace;
1576
1577 IMesh mesh = null;
1578
1579 if (_parent_scene.needsMeshing(_pbs))
1580 {
1581 // Don't need to re-enable body.. it's done in SetMesh
1582 mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
1583 // createmesh returns null when it's a shape that isn't a cube.
1584 // m_log.Debug(m_localID);
1585 if (mesh == null)
1586 CheckMeshAsset();
1587 else
1588 m_assetFailed = false;
1589 }
1590
1591#if SPAM
1592Console.WriteLine("changeadd 1");
1593#endif
1594 CreateGeom(m_targetSpace, mesh);
1595
1596 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
1597 d.Quaternion myrot = new d.Quaternion();
1598 myrot.X = _orientation.X;
1599 myrot.Y = _orientation.Y;
1600 myrot.Z = _orientation.Z;
1601 myrot.W = _orientation.W;
1602 d.GeomSetQuaternion(prim_geom, ref myrot);
1603
1604 if (IsPhysical && Body == IntPtr.Zero)
1605 enableBody();
1606
1607 changeSelectedStatus();
1608
1609 m_taintadd = false;
1610 }
1611
1612 /// <summary>
1613 /// Move prim in response to a move taint.
1614 /// </summary>
1615 private void changemove()
1616 {
1617 if (IsPhysical)
1618 {
1619 if (!m_disabled && !m_taintremove && !childPrim)
1620 {
1621 if (Body == IntPtr.Zero)
1622 enableBody();
1623
1624 //Prim auto disable after 20 frames,
1625 //if you move it, re-enable the prim manually.
1626 if (_parent != null)
1627 {
1628 if (m_linkJoint != IntPtr.Zero)
1629 {
1630 d.JointDestroy(m_linkJoint);
1631 m_linkJoint = IntPtr.Zero;
1632 }
1633 }
1634
1635 if (Body != IntPtr.Zero)
1636 {
1637 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
1638
1639 if (_parent != null)
1640 {
1641 OdePrim odParent = (OdePrim)_parent;
1642 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
1643 {
1644// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
1645Console.WriteLine(" JointCreateFixed");
1646 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
1647 d.JointAttach(m_linkJoint, Body, odParent.Body);
1648 d.JointSetFixed(m_linkJoint);
1649 }
1650 }
1651 d.BodyEnable(Body);
1652 if (m_vehicle.Type != Vehicle.TYPE_NONE)
1653 {
1654 m_vehicle.Enable(Body, _parent_scene);
1655 }
1656 }
1657 else
1658 {
1659 m_log.WarnFormat("[PHYSICS]: Body for {0} still null after enableBody(). This is a crash scenario.", Name);
1660 }
1661 }
1662 //else
1663 // {
1664 //m_log.Debug("[BUG]: race!");
1665 //}
1666 }
1667
1668 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
1669 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
1670// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1671
1672 IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace);
1673 m_targetSpace = tempspace;
1674
1675// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1676
1677 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
1678
1679// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1680 d.SpaceAdd(m_targetSpace, prim_geom);
1681
1682 changeSelectedStatus();
1683
1684 resetCollisionAccounting();
1685 m_taintposition = _position;
1686 }
1687
1688 internal void Move(float timestep)
1689 {
1690 float fx = 0;
1691 float fy = 0;
1692 float fz = 0;
1693
1694 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
1695 {
1696 if (m_vehicle.Type != Vehicle.TYPE_NONE)
1697 {
1698 // 'VEHICLES' are dealt with in ODEDynamics.cs
1699 m_vehicle.Step(timestep, _parent_scene);
1700 }
1701 else
1702 {
1703//Console.WriteLine("Move " + Name);
1704 if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
1705 // NON-'VEHICLES' are dealt with here
1706// if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f))
1707// {
1708// d.Vector3 avel2 = d.BodyGetAngularVel(Body);
1709// /*
1710// if (m_angularlock.X == 1)
1711// avel2.X = 0;
1712// if (m_angularlock.Y == 1)
1713// avel2.Y = 0;
1714// if (m_angularlock.Z == 1)
1715// avel2.Z = 0;
1716// d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
1717// */
1718// }
1719 //float PID_P = 900.0f;
1720
1721 float m_mass = CalculateMass();
1722
1723// fz = 0f;
1724 //m_log.Info(m_collisionFlags.ToString());
1725
1726
1727 //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
1728 // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ??
1729 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
1730 // gravityz multiplier = 1 - m_buoyancy
1731 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass;
1732
1733 if (PIDActive)
1734 {
1735//Console.WriteLine("PID " + Name);
1736 // KF - this is for object move? eg. llSetPos() ?
1737 //if (!d.BodyIsEnabled(Body))
1738 //d.BodySetForce(Body, 0f, 0f, 0f);
1739 // If we're using the PID controller, then we have no gravity
1740 //fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply...
1741 fz = 0f;
1742
1743 // no lock; for now it's only called from within Simulate()
1744
1745 // If the PID Controller isn't active then we set our force
1746 // calculating base velocity to the current position
1747
1748 if ((m_PIDTau < 1) && (m_PIDTau != 0))
1749 {
1750 //PID_G = PID_G / m_PIDTau;
1751 m_PIDTau = 1;
1752 }
1753
1754 if ((PID_G - m_PIDTau) <= 0)
1755 {
1756 PID_G = m_PIDTau + 1;
1757 }
1758 //PidStatus = true;
1759
1760 // PhysicsVector vec = new PhysicsVector();
1761 d.Vector3 vel = d.BodyGetLinearVel(Body);
1762
1763 d.Vector3 pos = d.BodyGetPosition(Body);
1764 _target_velocity =
1765 new Vector3(
1766 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
1767 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
1768 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
1769 );
1770
1771 // if velocity is zero, use position control; otherwise, velocity control
1772
1773 if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
1774 {
1775 // keep track of where we stopped. No more slippin' & slidin'
1776
1777 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1778 // react to the physics scene by moving it's position.
1779 // Avatar to Avatar collisions
1780 // Prim to avatar collisions
1781
1782 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
1783 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
1784 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
1785 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
1786 d.BodySetLinearVel(Body, 0, 0, 0);
1787 d.BodyAddForce(Body, 0, 0, fz);
1788 return;
1789 }
1790 else
1791 {
1792 _zeroFlag = false;
1793
1794 // We're flying and colliding with something
1795 fx = ((_target_velocity.X) - vel.X) * (PID_D);
1796 fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
1797
1798 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
1799
1800 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1801 }
1802 } // end if (PIDActive)
1803
1804 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
1805 if (m_useHoverPID && !PIDActive)
1806 {
1807//Console.WriteLine("Hover " + Name);
1808
1809 // If we're using the PID controller, then we have no gravity
1810 fz = (-1 * _parent_scene.gravityz) * m_mass;
1811
1812 // no lock; for now it's only called from within Simulate()
1813
1814 // If the PID Controller isn't active then we set our force
1815 // calculating base velocity to the current position
1816
1817 if ((m_PIDTau < 1))
1818 {
1819 PID_G = PID_G / m_PIDTau;
1820 }
1821
1822 if ((PID_G - m_PIDTau) <= 0)
1823 {
1824 PID_G = m_PIDTau + 1;
1825 }
1826
1827 // Where are we, and where are we headed?
1828 d.Vector3 pos = d.BodyGetPosition(Body);
1829 d.Vector3 vel = d.BodyGetLinearVel(Body);
1830
1831 // Non-Vehicles have a limited set of Hover options.
1832 // determine what our target height really is based on HoverType
1833 switch (m_PIDHoverType)
1834 {
1835 case PIDHoverType.Ground:
1836 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1837 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1838 break;
1839 case PIDHoverType.GroundAndWater:
1840 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1841 m_waterHeight = _parent_scene.GetWaterLevel();
1842 if (m_groundHeight > m_waterHeight)
1843 {
1844 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1845 }
1846 else
1847 {
1848 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
1849 }
1850 break;
1851
1852 } // end switch (m_PIDHoverType)
1853
1854
1855 _target_velocity =
1856 new Vector3(0.0f, 0.0f,
1857 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
1858 );
1859
1860 // if velocity is zero, use position control; otherwise, velocity control
1861
1862 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
1863 {
1864 // keep track of where we stopped. No more slippin' & slidin'
1865
1866 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1867 // react to the physics scene by moving it's position.
1868 // Avatar to Avatar collisions
1869 // Prim to avatar collisions
1870
1871 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
1872 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
1873 d.BodyAddForce(Body, 0, 0, fz);
1874 return;
1875 }
1876 else
1877 {
1878 _zeroFlag = false;
1879
1880 // We're flying and colliding with something
1881 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1882 }
1883 }
1884
1885 fx *= m_mass;
1886 fy *= m_mass;
1887 //fz *= m_mass;
1888
1889 fx += m_force.X;
1890 fy += m_force.Y;
1891 fz += m_force.Z;
1892
1893 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
1894 if (fx != 0 || fy != 0 || fz != 0)
1895 {
1896 //m_taintdisable = true;
1897 //base.RaiseOutOfBounds(Position);
1898 //d.BodySetLinearVel(Body, fx, fy, 0f);
1899 if (!d.BodyIsEnabled(Body))
1900 {
1901 // A physical body at rest on a surface will auto-disable after a while,
1902 // this appears to re-enable it incase the surface it is upon vanishes,
1903 // and the body should fall again.
1904 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1905 d.BodySetForce(Body, 0, 0, 0);
1906 enableBodySoft();
1907 }
1908
1909 // 35x10 = 350n times the mass per second applied maximum.
1910 float nmax = 35f * m_mass;
1911 float nmin = -35f * m_mass;
1912
1913 if (fx > nmax)
1914 fx = nmax;
1915 if (fx < nmin)
1916 fx = nmin;
1917 if (fy > nmax)
1918 fy = nmax;
1919 if (fy < nmin)
1920 fy = nmin;
1921 d.BodyAddForce(Body, fx, fy, fz);
1922//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
1923 }
1924 }
1925 }
1926 else
1927 { // is not physical, or is not a body or is selected
1928 // _zeroPosition = d.BodyGetPosition(Body);
1929 return;
1930//Console.WriteLine("Nothing " + Name);
1931
1932 }
1933 }
1934
1935 private void rotate()
1936 {
1937 d.Quaternion myrot = new d.Quaternion();
1938 myrot.X = _orientation.X;
1939 myrot.Y = _orientation.Y;
1940 myrot.Z = _orientation.Z;
1941 myrot.W = _orientation.W;
1942 if (Body != IntPtr.Zero)
1943 {
1944 // KF: If this is a root prim do BodySet
1945 d.BodySetQuaternion(Body, ref myrot);
1946 if (IsPhysical)
1947 {
1948 if (!m_angularlock.ApproxEquals(Vector3.One, 0f))
1949 createAMotor(m_angularlock);
1950 }
1951 }
1952 else
1953 {
1954 // daughter prim, do Geom set
1955 d.GeomSetQuaternion(prim_geom, ref myrot);
1956 }
1957
1958 resetCollisionAccounting();
1959 m_taintrot = _orientation;
1960 }
1961
1962 private void resetCollisionAccounting()
1963 {
1964 m_collisionscore = 0;
1965 m_interpenetrationcount = 0;
1966 m_disabled = false;
1967 }
1968
1969 /// <summary>
1970 /// Change prim in response to a disable taint.
1971 /// </summary>
1972 private void changedisable()
1973 {
1974 m_disabled = true;
1975 if (Body != IntPtr.Zero)
1976 {
1977 d.BodyDisable(Body);
1978 Body = IntPtr.Zero;
1979 }
1980
1981 m_taintdisable = false;
1982 }
1983
1984 /// <summary>
1985 /// Change prim in response to a physics status taint
1986 /// </summary>
1987 private void changePhysicsStatus()
1988 {
1989 if (IsPhysical)
1990 {
1991 if (Body == IntPtr.Zero)
1992 {
1993 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
1994 {
1995 changeshape();
1996 }
1997 else
1998 {
1999 enableBody();
2000 }
2001 }
2002 }
2003 else
2004 {
2005 if (Body != IntPtr.Zero)
2006 {
2007 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
2008 {
2009 RemoveGeom();
2010
2011//Console.WriteLine("changePhysicsStatus for " + Name);
2012 changeadd();
2013 }
2014
2015 if (childPrim)
2016 {
2017 if (_parent != null)
2018 {
2019 OdePrim parent = (OdePrim)_parent;
2020 parent.ChildDelink(this);
2021 }
2022 }
2023 else
2024 {
2025 disableBody();
2026 }
2027 }
2028 }
2029
2030 changeSelectedStatus();
2031
2032 resetCollisionAccounting();
2033 m_taintPhysics = IsPhysical;
2034 }
2035
2036 /// <summary>
2037 /// Change prim in response to a size taint.
2038 /// </summary>
2039 private void changesize()
2040 {
2041#if SPAM
2042 m_log.DebugFormat("[ODE PRIM]: Called changesize");
2043#endif
2044
2045 if (_size.X <= 0) _size.X = 0.01f;
2046 if (_size.Y <= 0) _size.Y = 0.01f;
2047 if (_size.Z <= 0) _size.Z = 0.01f;
2048
2049 //kill body to rebuild
2050 if (IsPhysical && Body != IntPtr.Zero)
2051 {
2052 if (childPrim)
2053 {
2054 if (_parent != null)
2055 {
2056 OdePrim parent = (OdePrim)_parent;
2057 parent.ChildDelink(this);
2058 }
2059 }
2060 else
2061 {
2062 disableBody();
2063 }
2064 }
2065
2066 if (d.SpaceQuery(m_targetSpace, prim_geom))
2067 {
2068// _parent_scene.waitForSpaceUnlock(m_targetSpace);
2069 d.SpaceRemove(m_targetSpace, prim_geom);
2070 }
2071
2072 RemoveGeom();
2073
2074 // we don't need to do space calculation because the client sends a position update also.
2075
2076 IMesh mesh = null;
2077
2078 // Construction of new prim
2079 if (_parent_scene.needsMeshing(_pbs))
2080 {
2081 float meshlod = _parent_scene.meshSculptLOD;
2082
2083 if (IsPhysical)
2084 meshlod = _parent_scene.MeshSculptphysicalLOD;
2085 // Don't need to re-enable body.. it's done in SetMesh
2086
2087 if (_parent_scene.needsMeshing(_pbs))
2088 {
2089 mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical);
2090 if (mesh == null)
2091 CheckMeshAsset();
2092 else
2093 m_assetFailed = false;
2094 }
2095
2096 }
2097
2098 CreateGeom(m_targetSpace, mesh);
2099 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2100 d.Quaternion myrot = new d.Quaternion();
2101 myrot.X = _orientation.X;
2102 myrot.Y = _orientation.Y;
2103 myrot.Z = _orientation.Z;
2104 myrot.W = _orientation.W;
2105 d.GeomSetQuaternion(prim_geom, ref myrot);
2106
2107 //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
2108 if (IsPhysical && Body == IntPtr.Zero && !childPrim)
2109 {
2110 // Re creates body on size.
2111 // EnableBody also does setMass()
2112 enableBody();
2113 d.BodyEnable(Body);
2114 }
2115
2116 changeSelectedStatus();
2117
2118 if (childPrim)
2119 {
2120 if (_parent is OdePrim)
2121 {
2122 OdePrim parent = (OdePrim)_parent;
2123 parent.ChildSetGeom(this);
2124 }
2125 }
2126 resetCollisionAccounting();
2127 m_taintsize = _size;
2128 }
2129
2130 /// <summary>
2131 /// Change prim in response to a float on water taint.
2132 /// </summary>
2133 /// <param name="timestep"></param>
2134 private void changefloatonwater()
2135 {
2136 m_collidesWater = m_taintCollidesWater;
2137
2138 if (m_collidesWater)
2139 {
2140 m_collisionFlags |= CollisionCategories.Water;
2141 }
2142 else
2143 {
2144 m_collisionFlags &= ~CollisionCategories.Water;
2145 }
2146
2147 if (m_assetFailed)
2148 d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
2149 else
2150
2151 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2152 }
2153 /// <summary>
2154 /// Change prim in response to a shape taint.
2155 /// </summary>
2156 private void changeshape()
2157 {
2158 m_taintshape = false;
2159
2160 // Cleanup of old prim geometry and Bodies
2161 if (IsPhysical && Body != IntPtr.Zero)
2162 {
2163 if (childPrim)
2164 {
2165 if (_parent != null)
2166 {
2167 OdePrim parent = (OdePrim)_parent;
2168 parent.ChildDelink(this);
2169 }
2170 }
2171 else
2172 {
2173 disableBody();
2174 }
2175 }
2176
2177 RemoveGeom();
2178
2179 // we don't need to do space calculation because the client sends a position update also.
2180 if (_size.X <= 0) _size.X = 0.01f;
2181 if (_size.Y <= 0) _size.Y = 0.01f;
2182 if (_size.Z <= 0) _size.Z = 0.01f;
2183 // Construction of new prim
2184
2185 IMesh mesh = null;
2186
2187
2188 if (_parent_scene.needsMeshing(_pbs))
2189 {
2190 // Don't need to re-enable body.. it's done in CreateMesh
2191 float meshlod = _parent_scene.meshSculptLOD;
2192
2193 if (IsPhysical)
2194 meshlod = _parent_scene.MeshSculptphysicalLOD;
2195
2196 // createmesh returns null when it doesn't mesh.
2197 mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical);
2198 if (mesh == null)
2199 CheckMeshAsset();
2200 else
2201 m_assetFailed = false;
2202 }
2203
2204 CreateGeom(m_targetSpace, mesh);
2205 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2206 d.Quaternion myrot = new d.Quaternion();
2207 //myrot.W = _orientation.w;
2208 myrot.W = _orientation.W;
2209 myrot.X = _orientation.X;
2210 myrot.Y = _orientation.Y;
2211 myrot.Z = _orientation.Z;
2212 d.GeomSetQuaternion(prim_geom, ref myrot);
2213
2214 //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
2215 if (IsPhysical && Body == IntPtr.Zero)
2216 {
2217 // Re creates body on size.
2218 // EnableBody also does setMass()
2219 enableBody();
2220 if (Body != IntPtr.Zero)
2221 {
2222 d.BodyEnable(Body);
2223 }
2224 }
2225
2226 changeSelectedStatus();
2227
2228 if (childPrim)
2229 {
2230 if (_parent is OdePrim)
2231 {
2232 OdePrim parent = (OdePrim)_parent;
2233 parent.ChildSetGeom(this);
2234 }
2235 }
2236
2237 resetCollisionAccounting();
2238// m_taintshape = false;
2239 }
2240
2241 /// <summary>
2242 /// Change prim in response to an add force taint.
2243 /// </summary>
2244 private void changeAddForce()
2245 {
2246 if (!m_isSelected)
2247 {
2248 lock (m_forcelist)
2249 {
2250 //m_log.Info("[PHYSICS]: dequeing forcelist");
2251 if (IsPhysical)
2252 {
2253 Vector3 iforce = Vector3.Zero;
2254 int i = 0;
2255 try
2256 {
2257 for (i = 0; i < m_forcelist.Count; i++)
2258 {
2259
2260 iforce = iforce + (m_forcelist[i] * 100);
2261 }
2262 }
2263 catch (IndexOutOfRangeException)
2264 {
2265 m_forcelist = new List<Vector3>();
2266 m_collisionscore = 0;
2267 m_interpenetrationcount = 0;
2268 m_taintforce = false;
2269 return;
2270 }
2271 catch (ArgumentOutOfRangeException)
2272 {
2273 m_forcelist = new List<Vector3>();
2274 m_collisionscore = 0;
2275 m_interpenetrationcount = 0;
2276 m_taintforce = false;
2277 return;
2278 }
2279 d.BodyEnable(Body);
2280 d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z);
2281 }
2282 m_forcelist.Clear();
2283 }
2284
2285 m_collisionscore = 0;
2286 m_interpenetrationcount = 0;
2287 }
2288
2289 m_taintforce = false;
2290 }
2291
2292 /// <summary>
2293 /// Change prim in response to a torque taint.
2294 /// </summary>
2295 private void changeSetTorque()
2296 {
2297 if (!m_isSelected)
2298 {
2299 if (IsPhysical && Body != IntPtr.Zero)
2300 {
2301 d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z);
2302 }
2303 }
2304
2305 m_taintTorque = Vector3.Zero;
2306 }
2307
2308 /// <summary>
2309 /// Change prim in response to an angular force taint.
2310 /// </summary>
2311 private void changeAddAngularForce()
2312 {
2313 if (!m_isSelected)
2314 {
2315 lock (m_angularforcelist)
2316 {
2317 //m_log.Info("[PHYSICS]: dequeing forcelist");
2318 if (IsPhysical)
2319 {
2320 Vector3 iforce = Vector3.Zero;
2321 for (int i = 0; i < m_angularforcelist.Count; i++)
2322 {
2323 iforce = iforce + (m_angularforcelist[i] * 100);
2324 }
2325 d.BodyEnable(Body);
2326 d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z);
2327
2328 }
2329 m_angularforcelist.Clear();
2330 }
2331
2332 m_collisionscore = 0;
2333 m_interpenetrationcount = 0;
2334 }
2335
2336 m_taintaddangularforce = false;
2337 }
2338
2339 /// <summary>
2340 /// Change prim in response to a velocity taint.
2341 /// </summary>
2342 private void changevelocity()
2343 {
2344 if (!m_isSelected)
2345 {
2346 // Not sure exactly why this sleep is here, but from experimentation it appears to stop an avatar
2347 // walking through a default rez size prim if it keeps kicking it around - justincc.
2348 Thread.Sleep(20);
2349
2350 if (IsPhysical)
2351 {
2352 if (Body != IntPtr.Zero)
2353 {
2354 d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
2355 }
2356 }
2357
2358 //resetCollisionAccounting();
2359 }
2360
2361 m_taintVelocity = Vector3.Zero;
2362 }
2363
2364 internal void setPrimForRemoval()
2365 {
2366 m_taintremove = true;
2367 }
2368
2369 public override bool Flying
2370 {
2371 // no flying prims for you
2372 get { return false; }
2373 set { }
2374 }
2375
2376 public override bool IsColliding
2377 {
2378 get { return iscolliding; }
2379 set { iscolliding = value; }
2380 }
2381
2382 public override bool CollidingGround
2383 {
2384 get { return false; }
2385 set { return; }
2386 }
2387
2388 public override bool CollidingObj
2389 {
2390 get { return false; }
2391 set { return; }
2392 }
2393
2394 public override bool ThrottleUpdates
2395 {
2396 get { return m_throttleUpdates; }
2397 set { m_throttleUpdates = value; }
2398 }
2399
2400 public override bool Stopped
2401 {
2402 get { return _zeroFlag; }
2403 }
2404
2405 public override Vector3 Position
2406 {
2407 get { return _position; }
2408
2409 set { _position = value;
2410 //m_log.Info("[PHYSICS]: " + _position.ToString());
2411 }
2412 }
2413
2414 public override Vector3 Size
2415 {
2416 get { return _size; }
2417 set
2418 {
2419 if (value.IsFinite())
2420 {
2421 _size = value;
2422// m_log.DebugFormat("[PHYSICS]: Set size on {0} to {1}", Name, value);
2423 }
2424 else
2425 {
2426 m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name);
2427 }
2428 }
2429 }
2430
2431 public override float Mass
2432 {
2433 get { return CalculateMass(); }
2434 }
2435
2436 public override Vector3 Force
2437 {
2438 //get { return Vector3.Zero; }
2439 get { return m_force; }
2440 set
2441 {
2442 if (value.IsFinite())
2443 {
2444 m_force = value;
2445 }
2446 else
2447 {
2448 m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name);
2449 }
2450 }
2451 }
2452
2453 public override int VehicleType
2454 {
2455 get { return (int)m_vehicle.Type; }
2456 set { m_vehicle.ProcessTypeChange((Vehicle)value); }
2457 }
2458
2459 public override void VehicleFloatParam(int param, float value)
2460 {
2461 m_vehicle.ProcessFloatVehicleParam((Vehicle) param, value);
2462 }
2463
2464 public override void VehicleVectorParam(int param, Vector3 value)
2465 {
2466 m_vehicle.ProcessVectorVehicleParam((Vehicle) param, value);
2467 }
2468
2469 public override void VehicleRotationParam(int param, Quaternion rotation)
2470 {
2471 m_vehicle.ProcessRotationVehicleParam((Vehicle) param, rotation);
2472 }
2473
2474 public override void VehicleFlags(int param, bool remove)
2475 {
2476 m_vehicle.ProcessVehicleFlags(param, remove);
2477 }
2478
2479 public override void SetVolumeDetect(int param)
2480 {
2481 // We have to lock the scene here so that an entire simulate loop either uses volume detect for all
2482 // possible collisions with this prim or for none of them.
2483 lock (_parent_scene.OdeLock)
2484 {
2485 m_isVolumeDetect = (param != 0);
2486 }
2487 }
2488
2489 public override Vector3 CenterOfMass
2490 {
2491 get { return Vector3.Zero; }
2492 }
2493
2494 public override Vector3 GeometricCenter
2495 {
2496 get { return Vector3.Zero; }
2497 }
2498
2499 public override PrimitiveBaseShape Shape
2500 {
2501 set
2502 {
2503 _pbs = value;
2504 m_assetFailed = false;
2505 m_taintshape = true;
2506 }
2507 }
2508
2509 public override Vector3 Velocity
2510 {
2511 get
2512 {
2513 // Average previous velocity with the new one so
2514 // client object interpolation works a 'little' better
2515 if (_zeroFlag)
2516 return Vector3.Zero;
2517
2518 Vector3 returnVelocity = Vector3.Zero;
2519 returnVelocity.X = (m_lastVelocity.X + _velocity.X) * 0.5f; // 0.5f is mathematically equiv to '/ 2'
2520 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) * 0.5f;
2521 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) * 0.5f;
2522 return returnVelocity;
2523 }
2524 set
2525 {
2526 if (value.IsFinite())
2527 {
2528 _velocity = value;
2529
2530 m_taintVelocity = value;
2531 _parent_scene.AddPhysicsActorTaint(this);
2532 }
2533 else
2534 {
2535 m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name);
2536 }
2537
2538 }
2539 }
2540
2541 public override Vector3 Torque
2542 {
2543 get
2544 {
2545 if (!IsPhysical || Body == IntPtr.Zero)
2546 return Vector3.Zero;
2547
2548 return _torque;
2549 }
2550
2551 set
2552 {
2553 if (value.IsFinite())
2554 {
2555 m_taintTorque = value;
2556 _parent_scene.AddPhysicsActorTaint(this);
2557 }
2558 else
2559 {
2560 m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name);
2561 }
2562 }
2563 }
2564
2565 public override float CollisionScore
2566 {
2567 get { return m_collisionscore; }
2568 set { m_collisionscore = value; }
2569 }
2570
2571 public override bool Kinematic
2572 {
2573 get { return false; }
2574 set { }
2575 }
2576
2577 public override Quaternion Orientation
2578 {
2579 get { return _orientation; }
2580 set
2581 {
2582 if (QuaternionIsFinite(value))
2583 _orientation = value;
2584 else
2585 m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name);
2586 }
2587 }
2588
2589 private static bool QuaternionIsFinite(Quaternion q)
2590 {
2591 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
2592 return false;
2593 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
2594 return false;
2595 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
2596 return false;
2597 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
2598 return false;
2599 return true;
2600 }
2601
2602 public override Vector3 Acceleration
2603 {
2604 get { return _acceleration; }
2605 set { _acceleration = value; }
2606 }
2607
2608 public override void AddForce(Vector3 force, bool pushforce)
2609 {
2610 if (force.IsFinite())
2611 {
2612 lock (m_forcelist)
2613 m_forcelist.Add(force);
2614
2615 m_taintforce = true;
2616 }
2617 else
2618 {
2619 m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name);
2620 }
2621 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
2622 }
2623
2624 public override void AddAngularForce(Vector3 force, bool pushforce)
2625 {
2626 if (force.IsFinite())
2627 {
2628 m_angularforcelist.Add(force);
2629 m_taintaddangularforce = true;
2630 }
2631 else
2632 {
2633 m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name);
2634 }
2635 }
2636
2637 public override Vector3 RotationalVelocity
2638 {
2639 get
2640 {
2641 Vector3 pv = Vector3.Zero;
2642 if (_zeroFlag)
2643 return pv;
2644 m_lastUpdateSent = false;
2645
2646 if (m_rotationalVelocity.ApproxEquals(pv, 0.2f))
2647 return pv;
2648
2649 return m_rotationalVelocity;
2650 }
2651 set
2652 {
2653 if (value.IsFinite())
2654 {
2655 m_rotationalVelocity = value;
2656 setAngularVelocity(value.X, value.Y, value.Z);
2657 }
2658 else
2659 {
2660 m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name);
2661 }
2662 }
2663 }
2664
2665 public override void CrossingFailure()
2666 {
2667 m_crossingfailures++;
2668 if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
2669 {
2670 base.RaiseOutOfBounds(_position);
2671 return;
2672 }
2673 else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
2674 {
2675 m_log.Warn("[PHYSICS]: Too many crossing failures for: " + Name);
2676 }
2677 }
2678
2679 public override float Buoyancy
2680 {
2681 get { return m_buoyancy; }
2682 set { m_buoyancy = value; }
2683 }
2684
2685 public override void link(PhysicsActor obj)
2686 {
2687 m_taintparent = obj;
2688 }
2689
2690 public override void delink()
2691 {
2692 m_taintparent = null;
2693 }
2694
2695 public override void LockAngularMotion(Vector3 axis)
2696 {
2697 // reverse the zero/non zero values for ODE.
2698 if (axis.IsFinite())
2699 {
2700 axis.X = (axis.X > 0) ? 1f : 0f;
2701 axis.Y = (axis.Y > 0) ? 1f : 0f;
2702 axis.Z = (axis.Z > 0) ? 1f : 0f;
2703 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
2704 m_taintAngularLock = axis;
2705 }
2706 else
2707 {
2708 m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name);
2709 }
2710 }
2711
2712 internal void UpdatePositionAndVelocity()
2713 {
2714 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
2715 if (_parent == null)
2716 {
2717 Vector3 pv = Vector3.Zero;
2718 bool lastZeroFlag = _zeroFlag;
2719 float m_minvelocity = 0;
2720 if (Body != (IntPtr)0) // FIXME -> or if it is a joint
2721 {
2722 d.Vector3 vec = d.BodyGetPosition(Body);
2723 d.Quaternion ori = d.BodyGetQuaternion(Body);
2724 d.Vector3 vel = d.BodyGetLinearVel(Body);
2725 d.Vector3 rotvel = d.BodyGetAngularVel(Body);
2726 d.Vector3 torque = d.BodyGetTorque(Body);
2727 _torque = new Vector3(torque.X, torque.Y, torque.Z);
2728 Vector3 l_position = Vector3.Zero;
2729 Quaternion l_orientation = Quaternion.Identity;
2730
2731 // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!)
2732 //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
2733 //if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
2734 //if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
2735 //if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
2736
2737 m_lastposition = _position;
2738 m_lastorientation = _orientation;
2739
2740 l_position.X = vec.X;
2741 l_position.Y = vec.Y;
2742 l_position.Z = vec.Z;
2743 l_orientation.X = ori.X;
2744 l_orientation.Y = ori.Y;
2745 l_orientation.Z = ori.Z;
2746 l_orientation.W = ori.W;
2747
2748 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)
2749 {
2750 //base.RaiseOutOfBounds(l_position);
2751
2752 if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
2753 {
2754 _position = l_position;
2755 //_parent_scene.remActivePrim(this);
2756 if (_parent == null)
2757 base.RequestPhysicsterseUpdate();
2758 return;
2759 }
2760 else
2761 {
2762 if (_parent == null)
2763 base.RaiseOutOfBounds(l_position);
2764 return;
2765 }
2766 }
2767
2768 if (l_position.Z < 0)
2769 {
2770 // This is so prim that get lost underground don't fall forever and suck up
2771 //
2772 // Sim resources and memory.
2773 // Disables the prim's movement physics....
2774 // It's a hack and will generate a console message if it fails.
2775
2776 //IsPhysical = false;
2777 if (_parent == null)
2778 base.RaiseOutOfBounds(_position);
2779
2780 _acceleration.X = 0;
2781 _acceleration.Y = 0;
2782 _acceleration.Z = 0;
2783
2784 _velocity.X = 0;
2785 _velocity.Y = 0;
2786 _velocity.Z = 0;
2787 m_rotationalVelocity.X = 0;
2788 m_rotationalVelocity.Y = 0;
2789 m_rotationalVelocity.Z = 0;
2790
2791 if (_parent == null)
2792 base.RequestPhysicsterseUpdate();
2793
2794 m_throttleUpdates = false;
2795 throttleCounter = 0;
2796 _zeroFlag = true;
2797 //outofBounds = true;
2798 }
2799
2800 //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation));
2801//Console.WriteLine("Adiff " + Name + " = " + Adiff);
2802 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
2803 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
2804 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
2805// && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01))
2806 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large
2807 {
2808 _zeroFlag = true;
2809//Console.WriteLine("ZFT 2");
2810 m_throttleUpdates = false;
2811 }
2812 else
2813 {
2814 //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
2815 _zeroFlag = false;
2816 m_lastUpdateSent = false;
2817 //m_throttleUpdates = false;
2818 }
2819
2820 if (_zeroFlag)
2821 {
2822 _velocity.X = 0.0f;
2823 _velocity.Y = 0.0f;
2824 _velocity.Z = 0.0f;
2825
2826 _acceleration.X = 0;
2827 _acceleration.Y = 0;
2828 _acceleration.Z = 0;
2829
2830 //_orientation.w = 0f;
2831 //_orientation.X = 0f;
2832 //_orientation.Y = 0f;
2833 //_orientation.Z = 0f;
2834 m_rotationalVelocity.X = 0;
2835 m_rotationalVelocity.Y = 0;
2836 m_rotationalVelocity.Z = 0;
2837 if (!m_lastUpdateSent)
2838 {
2839 m_throttleUpdates = false;
2840 throttleCounter = 0;
2841 m_rotationalVelocity = pv;
2842
2843 if (_parent == null)
2844 {
2845 base.RequestPhysicsterseUpdate();
2846 }
2847
2848 m_lastUpdateSent = true;
2849 }
2850 }
2851 else
2852 {
2853 if (lastZeroFlag != _zeroFlag)
2854 {
2855 if (_parent == null)
2856 {
2857 base.RequestPhysicsterseUpdate();
2858 }
2859 }
2860
2861 m_lastVelocity = _velocity;
2862
2863 _position = l_position;
2864
2865 _velocity.X = vel.X;
2866 _velocity.Y = vel.Y;
2867 _velocity.Z = vel.Z;
2868
2869 _acceleration = ((_velocity - m_lastVelocity) / 0.1f);
2870 _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f);
2871 //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());
2872
2873 // Note here that linearvelocity is affecting angular velocity... so I'm guessing this is a vehicle specific thing...
2874 // it does make sense to do this for tiny little instabilities with physical prim, however 0.5m/frame is fairly large.
2875 // 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
2876 // adding these logical exclusion situations to maintain this where I think it was intended to be.
2877 if (m_throttleUpdates || PIDActive || (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) || (Amotor != IntPtr.Zero))
2878 {
2879 m_minvelocity = 0.5f;
2880 }
2881 else
2882 {
2883 m_minvelocity = 0.02f;
2884 }
2885
2886 if (_velocity.ApproxEquals(pv, m_minvelocity))
2887 {
2888 m_rotationalVelocity = pv;
2889 }
2890 else
2891 {
2892 m_rotationalVelocity = new Vector3(rotvel.X, rotvel.Y, rotvel.Z);
2893 }
2894
2895 //m_log.Debug("ODE: " + m_rotationalVelocity.ToString());
2896 _orientation.X = ori.X;
2897 _orientation.Y = ori.Y;
2898 _orientation.Z = ori.Z;
2899 _orientation.W = ori.W;
2900 m_lastUpdateSent = false;
2901 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
2902 {
2903 if (_parent == null)
2904 {
2905 base.RequestPhysicsterseUpdate();
2906 }
2907 }
2908 else
2909 {
2910 throttleCounter++;
2911 }
2912 }
2913 m_lastposition = l_position;
2914 }
2915 else
2916 {
2917 // Not a body.. so Make sure the client isn't interpolating
2918 _velocity.X = 0;
2919 _velocity.Y = 0;
2920 _velocity.Z = 0;
2921
2922 _acceleration.X = 0;
2923 _acceleration.Y = 0;
2924 _acceleration.Z = 0;
2925
2926 m_rotationalVelocity.X = 0;
2927 m_rotationalVelocity.Y = 0;
2928 m_rotationalVelocity.Z = 0;
2929 _zeroFlag = true;
2930 }
2931 }
2932 }
2933
2934 public override bool FloatOnWater
2935 {
2936 set {
2937 m_taintCollidesWater = value;
2938 _parent_scene.AddPhysicsActorTaint(this);
2939 }
2940 }
2941
2942 public override void SetMomentum(Vector3 momentum)
2943 {
2944 }
2945
2946 public override Vector3 PIDTarget
2947 {
2948 set
2949 {
2950 if (value.IsFinite())
2951 {
2952 m_PIDTarget = value;
2953 }
2954 else
2955 m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name);
2956 }
2957 }
2958 public override bool PIDActive { get; set; }
2959 public override float PIDTau { set { m_PIDTau = value; } }
2960
2961 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
2962 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
2963 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
2964 public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
2965
2966 public override Quaternion APIDTarget{ set { return; } }
2967
2968 public override bool APIDActive{ set { return; } }
2969
2970 public override float APIDStrength{ set { return; } }
2971
2972 public override float APIDDamping{ set { return; } }
2973
2974 private void createAMotor(Vector3 axis)
2975 {
2976 if (Body == IntPtr.Zero)
2977 return;
2978
2979 if (Amotor != IntPtr.Zero)
2980 {
2981 d.JointDestroy(Amotor);
2982 Amotor = IntPtr.Zero;
2983 }
2984
2985 float axisnum = 3;
2986
2987 axisnum = (axisnum - (axis.X + axis.Y + axis.Z));
2988
2989 // PhysicsVector totalSize = new PhysicsVector(_size.X, _size.Y, _size.Z);
2990
2991
2992 // Inverse Inertia Matrix, set the X, Y, and/r Z inertia to 0 then invert it again.
2993 d.Mass objMass;
2994 d.MassSetZero(out objMass);
2995 DMassCopy(ref pMass, ref objMass);
2996
2997 //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);
2998
2999 Matrix4 dMassMat = FromDMass(objMass);
3000
3001 Matrix4 mathmat = Inverse(dMassMat);
3002
3003 /*
3004 //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]);
3005
3006 mathmat = Inverse(mathmat);
3007
3008
3009 objMass = FromMatrix4(mathmat, ref objMass);
3010 //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);
3011
3012 mathmat = Inverse(mathmat);
3013 */
3014 if (axis.X == 0)
3015 {
3016 mathmat.M33 = 50.0000001f;
3017 //objMass.I.M22 = 0;
3018 }
3019 if (axis.Y == 0)
3020 {
3021 mathmat.M22 = 50.0000001f;
3022 //objMass.I.M11 = 0;
3023 }
3024 if (axis.Z == 0)
3025 {
3026 mathmat.M11 = 50.0000001f;
3027 //objMass.I.M00 = 0;
3028 }
3029
3030
3031
3032 mathmat = Inverse(mathmat);
3033 objMass = FromMatrix4(mathmat, ref objMass);
3034 //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);
3035
3036 //return;
3037 if (d.MassCheck(ref objMass))
3038 {
3039 d.BodySetMass(Body, ref objMass);
3040 }
3041 else
3042 {
3043 //m_log.Debug("[PHYSICS]: Mass invalid, ignoring");
3044 }
3045
3046 if (axisnum <= 0)
3047 return;
3048 // int dAMotorEuler = 1;
3049
3050 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
3051 d.JointAttach(Amotor, Body, IntPtr.Zero);
3052 d.JointSetAMotorMode(Amotor, 0);
3053
3054 d.JointSetAMotorNumAxes(Amotor,(int)axisnum);
3055 int i = 0;
3056
3057 if (axis.X == 0)
3058 {
3059 d.JointSetAMotorAxis(Amotor, i, 0, 1, 0, 0);
3060 i++;
3061 }
3062
3063 if (axis.Y == 0)
3064 {
3065 d.JointSetAMotorAxis(Amotor, i, 0, 0, 1, 0);
3066 i++;
3067 }
3068
3069 if (axis.Z == 0)
3070 {
3071 d.JointSetAMotorAxis(Amotor, i, 0, 0, 0, 1);
3072 i++;
3073 }
3074
3075 for (int j = 0; j < (int)axisnum; j++)
3076 {
3077 //d.JointSetAMotorAngle(Amotor, j, 0);
3078 }
3079
3080 //d.JointSetAMotorAngle(Amotor, 1, 0);
3081 //d.JointSetAMotorAngle(Amotor, 2, 0);
3082
3083 // These lowstops and high stops are effectively (no wiggle room)
3084 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0f);
3085 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f);
3086 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0f);
3087 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
3088 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
3089 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
3090 //d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 9000f);
3091 d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
3092 d.JointSetAMotorParam(Amotor, (int)dParam.FMax, Mass * 50f);//
3093 }
3094
3095 private Matrix4 FromDMass(d.Mass pMass)
3096 {
3097 Matrix4 obj;
3098 obj.M11 = pMass.I.M00;
3099 obj.M12 = pMass.I.M01;
3100 obj.M13 = pMass.I.M02;
3101 obj.M14 = 0;
3102 obj.M21 = pMass.I.M10;
3103 obj.M22 = pMass.I.M11;
3104 obj.M23 = pMass.I.M12;
3105 obj.M24 = 0;
3106 obj.M31 = pMass.I.M20;
3107 obj.M32 = pMass.I.M21;
3108 obj.M33 = pMass.I.M22;
3109 obj.M34 = 0;
3110 obj.M41 = 0;
3111 obj.M42 = 0;
3112 obj.M43 = 0;
3113 obj.M44 = 1;
3114 return obj;
3115 }
3116
3117 private d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
3118 {
3119 obj.I.M00 = pMat[0, 0];
3120 obj.I.M01 = pMat[0, 1];
3121 obj.I.M02 = pMat[0, 2];
3122 obj.I.M10 = pMat[1, 0];
3123 obj.I.M11 = pMat[1, 1];
3124 obj.I.M12 = pMat[1, 2];
3125 obj.I.M20 = pMat[2, 0];
3126 obj.I.M21 = pMat[2, 1];
3127 obj.I.M22 = pMat[2, 2];
3128 return obj;
3129 }
3130
3131 public override void SubscribeEvents(int ms)
3132 {
3133 m_eventsubscription = ms;
3134 _parent_scene.AddCollisionEventReporting(this);
3135 }
3136
3137 public override void UnSubscribeEvents()
3138 {
3139 _parent_scene.RemoveCollisionEventReporting(this);
3140 m_eventsubscription = 0;
3141 }
3142
3143 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
3144 {
3145 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
3146 }
3147
3148 public void SendCollisions()
3149 {
3150 if (m_collisionsOnPreviousFrame || CollisionEventsThisFrame.Count > 0)
3151 {
3152 base.SendCollisionUpdate(CollisionEventsThisFrame);
3153
3154 if (CollisionEventsThisFrame.Count > 0)
3155 {
3156 m_collisionsOnPreviousFrame = true;
3157 CollisionEventsThisFrame.Clear();
3158 }
3159 else
3160 {
3161 m_collisionsOnPreviousFrame = false;
3162 }
3163 }
3164 }
3165
3166 public override bool SubscribedEvents()
3167 {
3168 if (m_eventsubscription > 0)
3169 return true;
3170 return false;
3171 }
3172
3173 public static Matrix4 Inverse(Matrix4 pMat)
3174 {
3175 if (determinant3x3(pMat) == 0)
3176 {
3177 return Matrix4.Identity; // should probably throw an error. singluar matrix inverse not possible
3178 }
3179
3180 return (Adjoint(pMat) / determinant3x3(pMat));
3181 }
3182
3183 public static Matrix4 Adjoint(Matrix4 pMat)
3184 {
3185 Matrix4 adjointMatrix = new Matrix4();
3186 for (int i=0; i<4; i++)
3187 {
3188 for (int j=0; j<4; j++)
3189 {
3190 Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j)))));
3191 }
3192 }
3193
3194 adjointMatrix = Transpose(adjointMatrix);
3195 return adjointMatrix;
3196 }
3197
3198 public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol)
3199 {
3200 Matrix4 minor = new Matrix4();
3201 int m = 0, n = 0;
3202 for (int i = 0; i < 4; i++)
3203 {
3204 if (i == iRow)
3205 continue;
3206 n = 0;
3207 for (int j = 0; j < 4; j++)
3208 {
3209 if (j == iCol)
3210 continue;
3211 Matrix4SetValue(ref minor, m,n, matrix[i, j]);
3212 n++;
3213 }
3214 m++;
3215 }
3216
3217 return minor;
3218 }
3219
3220 public static Matrix4 Transpose(Matrix4 pMat)
3221 {
3222 Matrix4 transposeMatrix = new Matrix4();
3223 for (int i = 0; i < 4; i++)
3224 for (int j = 0; j < 4; j++)
3225 Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]);
3226 return transposeMatrix;
3227 }
3228
3229 public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val)
3230 {
3231 switch (r)
3232 {
3233 case 0:
3234 switch (c)
3235 {
3236 case 0:
3237 pMat.M11 = val;
3238 break;
3239 case 1:
3240 pMat.M12 = val;
3241 break;
3242 case 2:
3243 pMat.M13 = val;
3244 break;
3245 case 3:
3246 pMat.M14 = val;
3247 break;
3248 }
3249
3250 break;
3251 case 1:
3252 switch (c)
3253 {
3254 case 0:
3255 pMat.M21 = val;
3256 break;
3257 case 1:
3258 pMat.M22 = val;
3259 break;
3260 case 2:
3261 pMat.M23 = val;
3262 break;
3263 case 3:
3264 pMat.M24 = val;
3265 break;
3266 }
3267
3268 break;
3269 case 2:
3270 switch (c)
3271 {
3272 case 0:
3273 pMat.M31 = val;
3274 break;
3275 case 1:
3276 pMat.M32 = val;
3277 break;
3278 case 2:
3279 pMat.M33 = val;
3280 break;
3281 case 3:
3282 pMat.M34 = val;
3283 break;
3284 }
3285
3286 break;
3287 case 3:
3288 switch (c)
3289 {
3290 case 0:
3291 pMat.M41 = val;
3292 break;
3293 case 1:
3294 pMat.M42 = val;
3295 break;
3296 case 2:
3297 pMat.M43 = val;
3298 break;
3299 case 3:
3300 pMat.M44 = val;
3301 break;
3302 }
3303
3304 break;
3305 }
3306 }
3307
3308 private static float determinant3x3(Matrix4 pMat)
3309 {
3310 float det = 0;
3311 float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2];
3312 float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0];
3313 float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1];
3314 float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2];
3315 float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0];
3316 float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1];
3317
3318 det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6);
3319 return det;
3320 }
3321
3322 private static void DMassCopy(ref d.Mass src, ref d.Mass dst)
3323 {
3324 dst.c.W = src.c.W;
3325 dst.c.X = src.c.X;
3326 dst.c.Y = src.c.Y;
3327 dst.c.Z = src.c.Z;
3328 dst.mass = src.mass;
3329 dst.I.M00 = src.I.M00;
3330 dst.I.M01 = src.I.M01;
3331 dst.I.M02 = src.I.M02;
3332 dst.I.M10 = src.I.M10;
3333 dst.I.M11 = src.I.M11;
3334 dst.I.M12 = src.I.M12;
3335 dst.I.M20 = src.I.M20;
3336 dst.I.M21 = src.I.M21;
3337 dst.I.M22 = src.I.M22;
3338 }
3339
3340 public override void SetMaterial(int pMaterial)
3341 {
3342 m_material = pMaterial;
3343 }
3344
3345 private void CheckMeshAsset()
3346 {
3347 if (_pbs.SculptEntry && !m_assetFailed && _pbs.SculptTexture != UUID.Zero)
3348 {
3349 m_assetFailed = true;
3350 Util.FireAndForget(delegate
3351 {
3352 RequestAssetDelegate assetProvider = _parent_scene.RequestAssetMethod;
3353 if (assetProvider != null)
3354 assetProvider(_pbs.SculptTexture, MeshAssetReceived);
3355 }, null, "ODEPrim.CheckMeshAsset");
3356 }
3357 }
3358
3359 private void MeshAssetReceived(AssetBase asset)
3360 {
3361 if (asset != null && asset.Data != null && asset.Data.Length > 0)
3362 {
3363 if (!_pbs.SculptEntry)
3364 return;
3365 if (_pbs.SculptTexture.ToString() != asset.ID)
3366 return;
3367
3368 _pbs.SculptData = new byte[asset.Data.Length];
3369 asset.Data.CopyTo(_pbs.SculptData, 0);
3370// m_assetFailed = false;
3371
3372// m_log.DebugFormat(
3373// "[ODE PRIM]: Received mesh/sculpt data asset {0} with {1} bytes for {2} at {3} in {4}",
3374// _pbs.SculptTexture, _pbs.SculptData.Length, Name, _position, _parent_scene.Name);
3375
3376 m_taintshape = true;
3377 _parent_scene.AddPhysicsActorTaint(this);
3378 }
3379 else
3380 {
3381 m_log.WarnFormat(
3382 "[ODE PRIM]: Could not get mesh/sculpt asset {0} for {1} at {2} in {3}",
3383 _pbs.SculptTexture, Name, _position, _parent_scene.Name);
3384 }
3385 }
3386 }
3387} \ No newline at end of file
generated by cgit v1.1 at 2024-08-22 21:16:55 +0000