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.cs3435
1 files changed, 3435 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..54a2033
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs
@@ -0,0 +1,3435 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28/*
29 * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces
30 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
31 * ODEPrim.cs contains methods dealing with Prim editing, Prim
32 * characteristics and Kinetic motion.
33 * ODEDynamics.cs contains methods dealing with Prim Physical motion
34 * (dynamics) and the associated settings. Old Linear and angular
35 * motors for dynamic motion have been replace with MoveLinear()
36 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
37 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
38 * switch between 'VEHICLE' parameter use and general dynamics
39 * settings use.
40 */
41
42//#define SPAM
43
44using System;
45using System.Collections.Generic;
46using System.Reflection;
47using System.Runtime.InteropServices;
48using System.Threading;
49using log4net;
50using OpenMetaverse;
51using Ode.NET;
52using OpenSim.Framework;
53using OpenSim.Region.PhysicsModules.SharedBase;
54
55namespace OpenSim.Region.PhysicsModule.ODE
56{
57 /// <summary>
58 /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
59 /// </summary>
60 public class OdePrim : PhysicsActor
61 {
62 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
63
64 private bool m_isphysical;
65
66 public int ExpectedCollisionContacts { get { return m_expectedCollisionContacts; } }
67 private int m_expectedCollisionContacts = 0;
68
69 /// <summary>
70 /// Gets collide bits so that we can still perform land collisions if a mesh fails to load.
71 /// </summary>
72 private int BadMeshAssetCollideBits
73 {
74 get { return m_isphysical ? (int)CollisionCategories.Land : 0; }
75 }
76
77 /// <summary>
78 /// Is this prim subject to physics? Even if not, it's still solid for collision purposes.
79 /// </summary>
80 public override bool IsPhysical
81 {
82 get { return m_isphysical; }
83 set
84 {
85 m_isphysical = value;
86 if (!m_isphysical) // Zero the remembered last velocity
87 m_lastVelocity = Vector3.Zero;
88 }
89 }
90
91 private Vector3 _position;
92 private Vector3 _velocity;
93 private Vector3 _torque;
94 private Vector3 m_lastVelocity;
95 private Vector3 m_lastposition;
96 private Quaternion m_lastorientation = new Quaternion();
97 private Vector3 m_rotationalVelocity;
98 private Vector3 _size;
99 private Vector3 _acceleration;
100 // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f);
101 private Quaternion _orientation;
102 private Vector3 m_taintposition;
103 private Vector3 m_taintsize;
104 private Vector3 m_taintVelocity;
105 private Vector3 m_taintTorque;
106 private Quaternion m_taintrot;
107 private Vector3 m_angularlock = Vector3.One;
108 private Vector3 m_taintAngularLock = Vector3.One;
109 private IntPtr Amotor = IntPtr.Zero;
110
111 private bool m_assetFailed = false;
112
113 private Vector3 m_PIDTarget;
114 private float m_PIDTau;
115 private float PID_D = 35f;
116 private float PID_G = 25f;
117
118 // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
119 // and are for non-VEHICLES only.
120
121 private float m_PIDHoverHeight;
122 private float m_PIDHoverTau;
123 private bool m_useHoverPID;
124 private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
125 private float m_targetHoverHeight;
126 private float m_groundHeight;
127 private float m_waterHeight;
128 private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
129
130 // private float m_tensor = 5f;
131 private int body_autodisable_frames = 20;
132
133
134 private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
135 | CollisionCategories.Space
136 | CollisionCategories.Body
137 | CollisionCategories.Character
138 );
139 private bool m_taintshape;
140 private bool m_taintPhysics;
141 private bool m_collidesLand = true;
142 private bool m_collidesWater;
143
144 // Default we're a Geometry
145 private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
146
147 // Default, Collide with Other Geometries, spaces and Bodies
148 private CollisionCategories m_collisionFlags = m_default_collisionFlags;
149
150 public bool m_taintremove { get; private set; }
151 public bool m_taintdisable { get; private set; }
152 internal bool m_disabled;
153 public bool m_taintadd { get; private set; }
154 public bool m_taintselected { get; private set; }
155 public bool m_taintCollidesWater { get; private set; }
156
157 private bool m_taintforce = false;
158 private bool m_taintaddangularforce = false;
159 private Vector3 m_force;
160 private List<Vector3> m_forcelist = new List<Vector3>();
161 private List<Vector3> m_angularforcelist = new List<Vector3>();
162
163 private PrimitiveBaseShape _pbs;
164 private OdeScene _parent_scene;
165
166 /// <summary>
167 /// The physics space which contains prim geometries
168 /// </summary>
169 public IntPtr m_targetSpace = IntPtr.Zero;
170
171 /// <summary>
172 /// The prim geometry, used for collision detection.
173 /// </summary>
174 /// <remarks>
175 /// This is never null except for a brief period when the geometry needs to be replaced (due to resizing or
176 /// mesh change) or when the physical prim is being removed from the scene.
177 /// </remarks>
178 public IntPtr prim_geom { get; private set; }
179
180 public IntPtr _triMeshData { get; private set; }
181
182 private IntPtr _linkJointGroup = IntPtr.Zero;
183 private PhysicsActor _parent;
184 private PhysicsActor m_taintparent;
185
186 private List<OdePrim> childrenPrim = new List<OdePrim>();
187
188 private bool iscolliding;
189 private bool m_isSelected;
190
191 internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
192
193 private bool m_throttleUpdates;
194 private int throttleCounter;
195 public int m_interpenetrationcount { get; private set; }
196 internal float m_collisionscore;
197 public int m_roundsUnderMotionThreshold { get; private set; }
198 private int m_crossingfailures;
199
200 public bool outofBounds { get; private set; }
201 private float m_density = 10.000006836f; // Aluminum g/cm3;
202
203 public bool _zeroFlag { get; private set; }
204 private bool m_lastUpdateSent;
205
206 public IntPtr Body = IntPtr.Zero;
207 private Vector3 _target_velocity;
208 private d.Mass pMass;
209
210 private int m_eventsubscription;
211 private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
212
213 /// <summary>
214 /// Signal whether there were collisions on the previous frame, so we know if we need to send the
215 /// empty CollisionEventsThisFrame to the prim so that it can detect the end of a collision.
216 /// </summary>
217 /// <remarks>
218 /// This is probably a temporary measure, pending storing this information consistently in CollisionEventUpdate itself.
219 /// </remarks>
220 private bool m_collisionsOnPreviousFrame;
221
222 private IntPtr m_linkJoint = IntPtr.Zero;
223
224 internal volatile bool childPrim;
225
226 private ODEDynamics m_vehicle;
227
228 internal int m_material = (int)Material.Wood;
229
230 public OdePrim(
231 String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
232 Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical)
233 {
234 Name = primName;
235 m_vehicle = new ODEDynamics();
236 //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
237
238 if (!pos.IsFinite())
239 {
240 pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
241 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
242 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name);
243 }
244 _position = pos;
245 m_taintposition = pos;
246 PID_D = parent_scene.bodyPIDD;
247 PID_G = parent_scene.bodyPIDG;
248 m_density = parent_scene.geomDefaultDensity;
249 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
250 body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
251
252 prim_geom = IntPtr.Zero;
253
254 if (!pos.IsFinite())
255 {
256 size = new Vector3(0.5f, 0.5f, 0.5f);
257 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name);
258 }
259
260 if (size.X <= 0) size.X = 0.01f;
261 if (size.Y <= 0) size.Y = 0.01f;
262 if (size.Z <= 0) size.Z = 0.01f;
263
264 _size = size;
265 m_taintsize = _size;
266
267 if (!QuaternionIsFinite(rotation))
268 {
269 rotation = Quaternion.Identity;
270 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name);
271 }
272
273 _orientation = rotation;
274 m_taintrot = _orientation;
275 _pbs = pbs;
276
277 _parent_scene = parent_scene;
278 m_targetSpace = (IntPtr)0;
279
280 if (pos.Z < 0)
281 {
282 IsPhysical = false;
283 }
284 else
285 {
286 IsPhysical = pisPhysical;
287 // If we're physical, we need to be in the master space for now.
288 // linksets *should* be in a space together.. but are not currently
289 if (IsPhysical)
290 m_targetSpace = _parent_scene.space;
291 }
292
293 m_taintadd = true;
294 m_assetFailed = false;
295 _parent_scene.AddPhysicsActorTaint(this);
296 }
297
298 public override int PhysicsActorType
299 {
300 get { return (int) ActorTypes.Prim; }
301 set { return; }
302 }
303
304 public override bool SetAlwaysRun
305 {
306 get { return false; }
307 set { return; }
308 }
309
310 public override bool Grabbed
311 {
312 set { return; }
313 }
314
315 public override bool Selected
316 {
317 set
318 {
319 // This only makes the object not collidable if the object
320 // is physical or the object is modified somehow *IN THE FUTURE*
321 // without this, if an avatar selects prim, they can walk right
322 // through it while it's selected
323 m_collisionscore = 0;
324
325 if ((IsPhysical && !_zeroFlag) || !value)
326 {
327 m_taintselected = value;
328 _parent_scene.AddPhysicsActorTaint(this);
329 }
330 else
331 {
332 m_taintselected = value;
333 m_isSelected = value;
334 }
335
336 if (m_isSelected)
337 disableBodySoft();
338 }
339 }
340
341 /// <summary>
342 /// Set a new geometry for this prim.
343 /// </summary>
344 /// <param name="geom"></param>
345 private void SetGeom(IntPtr geom)
346 {
347 prim_geom = geom;
348//Console.WriteLine("SetGeom to " + prim_geom + " for " + Name);
349
350 if (m_assetFailed)
351 {
352 d.GeomSetCategoryBits(prim_geom, 0);
353 d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
354 }
355 else
356 {
357 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
358 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
359 }
360
361 _parent_scene.geom_name_map[prim_geom] = Name;
362 _parent_scene.actor_name_map[prim_geom] = this;
363
364 if (childPrim)
365 {
366 if (_parent != null && _parent is OdePrim)
367 {
368 OdePrim parent = (OdePrim)_parent;
369//Console.WriteLine("SetGeom calls ChildSetGeom");
370 parent.ChildSetGeom(this);
371 }
372 }
373 //m_log.Warn("Setting Geom to: " + prim_geom);
374 }
375
376 private void enableBodySoft()
377 {
378 if (!childPrim)
379 {
380 if (IsPhysical && Body != IntPtr.Zero)
381 {
382 d.BodyEnable(Body);
383 if (m_vehicle.Type != Vehicle.TYPE_NONE)
384 m_vehicle.Enable(Body, _parent_scene);
385 }
386
387 m_disabled = false;
388 }
389 }
390
391 private void disableBodySoft()
392 {
393 m_disabled = true;
394
395 if (IsPhysical && Body != IntPtr.Zero)
396 {
397 d.BodyDisable(Body);
398 }
399 }
400
401 /// <summary>
402 /// Make a prim subject to physics.
403 /// </summary>
404 private void enableBody()
405 {
406 // Don't enable this body if we're a child prim
407 // this should be taken care of in the parent function not here
408 if (!childPrim)
409 {
410 // Sets the geom to a body
411 Body = d.BodyCreate(_parent_scene.world);
412
413 setMass();
414 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
415 d.Quaternion myrot = new d.Quaternion();
416 myrot.X = _orientation.X;
417 myrot.Y = _orientation.Y;
418 myrot.Z = _orientation.Z;
419 myrot.W = _orientation.W;
420 d.BodySetQuaternion(Body, ref myrot);
421 d.GeomSetBody(prim_geom, Body);
422
423 if (m_assetFailed)
424 {
425 d.GeomSetCategoryBits(prim_geom, 0);
426 d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
427 }
428 else
429 {
430 m_collisionCategories |= CollisionCategories.Body;
431 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
432 }
433
434 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
435 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
436
437 d.BodySetAutoDisableFlag(Body, true);
438 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
439
440 // disconnect from world gravity so we can apply buoyancy
441 d.BodySetGravityMode (Body, false);
442
443 m_interpenetrationcount = 0;
444 m_collisionscore = 0;
445 m_disabled = false;
446
447 // The body doesn't already have a finite rotation mode set here
448 if ((!m_angularlock.ApproxEquals(Vector3.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 (outofBounds)
1695 return;
1696
1697 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
1698 {
1699 if (m_vehicle.Type != Vehicle.TYPE_NONE)
1700 {
1701 // 'VEHICLES' are dealt with in ODEDynamics.cs
1702 m_vehicle.Step(timestep, _parent_scene);
1703 }
1704 else
1705 {
1706//Console.WriteLine("Move " + Name);
1707 if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
1708 // NON-'VEHICLES' are dealt with here
1709// if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f))
1710// {
1711// d.Vector3 avel2 = d.BodyGetAngularVel(Body);
1712// /*
1713// if (m_angularlock.X == 1)
1714// avel2.X = 0;
1715// if (m_angularlock.Y == 1)
1716// avel2.Y = 0;
1717// if (m_angularlock.Z == 1)
1718// avel2.Z = 0;
1719// d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
1720// */
1721// }
1722 //float PID_P = 900.0f;
1723
1724 float m_mass = CalculateMass();
1725
1726// fz = 0f;
1727 //m_log.Info(m_collisionFlags.ToString());
1728
1729
1730 //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
1731 // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ??
1732 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
1733 // gravityz multiplier = 1 - m_buoyancy
1734 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass;
1735
1736 if (PIDActive)
1737 {
1738//Console.WriteLine("PID " + Name);
1739 // KF - this is for object move? eg. llSetPos() ?
1740 //if (!d.BodyIsEnabled(Body))
1741 //d.BodySetForce(Body, 0f, 0f, 0f);
1742 // If we're using the PID controller, then we have no gravity
1743 //fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply...
1744 fz = 0f;
1745
1746 // no lock; for now it's only called from within Simulate()
1747
1748 // If the PID Controller isn't active then we set our force
1749 // calculating base velocity to the current position
1750
1751 if ((m_PIDTau < 1) && (m_PIDTau != 0))
1752 {
1753 //PID_G = PID_G / m_PIDTau;
1754 m_PIDTau = 1;
1755 }
1756
1757 if ((PID_G - m_PIDTau) <= 0)
1758 {
1759 PID_G = m_PIDTau + 1;
1760 }
1761 //PidStatus = true;
1762
1763 // PhysicsVector vec = new PhysicsVector();
1764 d.Vector3 vel = d.BodyGetLinearVel(Body);
1765
1766 d.Vector3 pos = d.BodyGetPosition(Body);
1767 _target_velocity =
1768 new Vector3(
1769 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
1770 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
1771 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
1772 );
1773
1774 // if velocity is zero, use position control; otherwise, velocity control
1775
1776 if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
1777 {
1778 // keep track of where we stopped. No more slippin' & slidin'
1779
1780 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1781 // react to the physics scene by moving it's position.
1782 // Avatar to Avatar collisions
1783 // Prim to avatar collisions
1784
1785 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
1786 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
1787 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
1788 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
1789 d.BodySetLinearVel(Body, 0, 0, 0);
1790 d.BodyAddForce(Body, 0, 0, fz);
1791 return;
1792 }
1793 else
1794 {
1795 _zeroFlag = false;
1796
1797 // We're flying and colliding with something
1798 fx = ((_target_velocity.X) - vel.X) * (PID_D);
1799 fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
1800
1801 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
1802
1803 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1804 }
1805 } // end if (PIDActive)
1806
1807 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
1808 if (m_useHoverPID && !PIDActive)
1809 {
1810//Console.WriteLine("Hover " + Name);
1811
1812 // If we're using the PID controller, then we have no gravity
1813 fz = (-1 * _parent_scene.gravityz) * m_mass;
1814
1815 // no lock; for now it's only called from within Simulate()
1816
1817 // If the PID Controller isn't active then we set our force
1818 // calculating base velocity to the current position
1819
1820 if ((m_PIDTau < 1))
1821 {
1822 PID_G = PID_G / m_PIDTau;
1823 }
1824
1825 if ((PID_G - m_PIDTau) <= 0)
1826 {
1827 PID_G = m_PIDTau + 1;
1828 }
1829
1830 // Where are we, and where are we headed?
1831 d.Vector3 pos = d.BodyGetPosition(Body);
1832 d.Vector3 vel = d.BodyGetLinearVel(Body);
1833
1834 // Non-Vehicles have a limited set of Hover options.
1835 // determine what our target height really is based on HoverType
1836 switch (m_PIDHoverType)
1837 {
1838 case PIDHoverType.Ground:
1839 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1840 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1841 break;
1842 case PIDHoverType.GroundAndWater:
1843 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1844 m_waterHeight = _parent_scene.GetWaterLevel();
1845 if (m_groundHeight > m_waterHeight)
1846 {
1847 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1848 }
1849 else
1850 {
1851 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
1852 }
1853 break;
1854
1855 } // end switch (m_PIDHoverType)
1856
1857
1858 _target_velocity =
1859 new Vector3(0.0f, 0.0f,
1860 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
1861 );
1862
1863 // if velocity is zero, use position control; otherwise, velocity control
1864
1865 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
1866 {
1867 // keep track of where we stopped. No more slippin' & slidin'
1868
1869 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1870 // react to the physics scene by moving it's position.
1871 // Avatar to Avatar collisions
1872 // Prim to avatar collisions
1873
1874 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
1875 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
1876 d.BodyAddForce(Body, 0, 0, fz);
1877 return;
1878 }
1879 else
1880 {
1881 _zeroFlag = false;
1882
1883 // We're flying and colliding with something
1884 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1885 }
1886 }
1887
1888 fx *= m_mass;
1889 fy *= m_mass;
1890 //fz *= m_mass;
1891
1892 fx += m_force.X;
1893 fy += m_force.Y;
1894 fz += m_force.Z;
1895
1896 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
1897 if (fx != 0 || fy != 0 || fz != 0)
1898 {
1899 //m_taintdisable = true;
1900 //base.RaiseOutOfBounds(Position);
1901 //d.BodySetLinearVel(Body, fx, fy, 0f);
1902 if (!d.BodyIsEnabled(Body))
1903 {
1904 // A physical body at rest on a surface will auto-disable after a while,
1905 // this appears to re-enable it incase the surface it is upon vanishes,
1906 // and the body should fall again.
1907 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1908 d.BodySetForce(Body, 0, 0, 0);
1909 enableBodySoft();
1910 }
1911
1912 // 35x10 = 350n times the mass per second applied maximum.
1913 float nmax = 35f * m_mass;
1914 float nmin = -35f * m_mass;
1915
1916 if (fx > nmax)
1917 fx = nmax;
1918 if (fx < nmin)
1919 fx = nmin;
1920 if (fy > nmax)
1921 fy = nmax;
1922 if (fy < nmin)
1923 fy = nmin;
1924 d.BodyAddForce(Body, fx, fy, fz);
1925//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
1926 }
1927 }
1928 }
1929 else
1930 { // is not physical, or is not a body or is selected
1931 // _zeroPosition = d.BodyGetPosition(Body);
1932 return;
1933//Console.WriteLine("Nothing " + Name);
1934
1935 }
1936 }
1937
1938 private void rotate()
1939 {
1940 d.Quaternion myrot = new d.Quaternion();
1941 myrot.X = _orientation.X;
1942 myrot.Y = _orientation.Y;
1943 myrot.Z = _orientation.Z;
1944 myrot.W = _orientation.W;
1945 if (Body != IntPtr.Zero)
1946 {
1947 // KF: If this is a root prim do BodySet
1948 d.BodySetQuaternion(Body, ref myrot);
1949 if (IsPhysical)
1950 {
1951 if (!m_angularlock.ApproxEquals(Vector3.One, 0f))
1952 createAMotor(m_angularlock);
1953 }
1954 }
1955 else
1956 {
1957 // daughter prim, do Geom set
1958 d.GeomSetQuaternion(prim_geom, ref myrot);
1959 }
1960
1961 resetCollisionAccounting();
1962 m_taintrot = _orientation;
1963 }
1964
1965 private void resetCollisionAccounting()
1966 {
1967 m_collisionscore = 0;
1968 m_interpenetrationcount = 0;
1969 m_disabled = false;
1970 }
1971
1972 /// <summary>
1973 /// Change prim in response to a disable taint.
1974 /// </summary>
1975 private void changedisable()
1976 {
1977 m_disabled = true;
1978 if (Body != IntPtr.Zero)
1979 {
1980 d.BodyDisable(Body);
1981 Body = IntPtr.Zero;
1982 }
1983
1984 m_taintdisable = false;
1985 }
1986
1987 /// <summary>
1988 /// Change prim in response to a physics status taint
1989 /// </summary>
1990 private void changePhysicsStatus()
1991 {
1992 if (IsPhysical)
1993 {
1994 if (Body == IntPtr.Zero)
1995 {
1996 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
1997 {
1998 changeshape();
1999 }
2000 else
2001 {
2002 enableBody();
2003 }
2004 }
2005 }
2006 else
2007 {
2008 if (Body != IntPtr.Zero)
2009 {
2010 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
2011 {
2012 RemoveGeom();
2013
2014//Console.WriteLine("changePhysicsStatus for " + Name);
2015 changeadd();
2016 }
2017
2018 if (childPrim)
2019 {
2020 if (_parent != null)
2021 {
2022 OdePrim parent = (OdePrim)_parent;
2023 parent.ChildDelink(this);
2024 }
2025 }
2026 else
2027 {
2028 disableBody();
2029 }
2030 }
2031 }
2032
2033 changeSelectedStatus();
2034
2035 resetCollisionAccounting();
2036 m_taintPhysics = IsPhysical;
2037 }
2038
2039 /// <summary>
2040 /// Change prim in response to a size taint.
2041 /// </summary>
2042 private void changesize()
2043 {
2044#if SPAM
2045 m_log.DebugFormat("[ODE PRIM]: Called changesize");
2046#endif
2047
2048 if (_size.X <= 0) _size.X = 0.01f;
2049 if (_size.Y <= 0) _size.Y = 0.01f;
2050 if (_size.Z <= 0) _size.Z = 0.01f;
2051
2052 //kill body to rebuild
2053 if (IsPhysical && Body != IntPtr.Zero)
2054 {
2055 if (childPrim)
2056 {
2057 if (_parent != null)
2058 {
2059 OdePrim parent = (OdePrim)_parent;
2060 parent.ChildDelink(this);
2061 }
2062 }
2063 else
2064 {
2065 disableBody();
2066 }
2067 }
2068
2069 if (d.SpaceQuery(m_targetSpace, prim_geom))
2070 {
2071// _parent_scene.waitForSpaceUnlock(m_targetSpace);
2072 d.SpaceRemove(m_targetSpace, prim_geom);
2073 }
2074
2075 RemoveGeom();
2076
2077 // we don't need to do space calculation because the client sends a position update also.
2078
2079 IMesh mesh = null;
2080
2081 // Construction of new prim
2082 if (_parent_scene.needsMeshing(_pbs))
2083 {
2084 float meshlod = _parent_scene.meshSculptLOD;
2085
2086 if (IsPhysical)
2087 meshlod = _parent_scene.MeshSculptphysicalLOD;
2088 // Don't need to re-enable body.. it's done in SetMesh
2089
2090 if (_parent_scene.needsMeshing(_pbs))
2091 {
2092 mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical);
2093 if (mesh == null)
2094 CheckMeshAsset();
2095 else
2096 m_assetFailed = false;
2097 }
2098
2099 }
2100
2101 CreateGeom(m_targetSpace, mesh);
2102 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2103 d.Quaternion myrot = new d.Quaternion();
2104 myrot.X = _orientation.X;
2105 myrot.Y = _orientation.Y;
2106 myrot.Z = _orientation.Z;
2107 myrot.W = _orientation.W;
2108 d.GeomSetQuaternion(prim_geom, ref myrot);
2109
2110 //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
2111 if (IsPhysical && Body == IntPtr.Zero && !childPrim)
2112 {
2113 // Re creates body on size.
2114 // EnableBody also does setMass()
2115 enableBody();
2116 d.BodyEnable(Body);
2117 }
2118
2119 changeSelectedStatus();
2120
2121 if (childPrim)
2122 {
2123 if (_parent is OdePrim)
2124 {
2125 OdePrim parent = (OdePrim)_parent;
2126 parent.ChildSetGeom(this);
2127 }
2128 }
2129 resetCollisionAccounting();
2130 m_taintsize = _size;
2131 }
2132
2133 /// <summary>
2134 /// Change prim in response to a float on water taint.
2135 /// </summary>
2136 /// <param name="timestep"></param>
2137 private void changefloatonwater()
2138 {
2139 m_collidesWater = m_taintCollidesWater;
2140
2141 if (m_collidesWater)
2142 {
2143 m_collisionFlags |= CollisionCategories.Water;
2144 }
2145 else
2146 {
2147 m_collisionFlags &= ~CollisionCategories.Water;
2148 }
2149
2150 if (m_assetFailed)
2151 d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
2152 else
2153
2154 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2155 }
2156 /// <summary>
2157 /// Change prim in response to a shape taint.
2158 /// </summary>
2159 private void changeshape()
2160 {
2161 m_taintshape = false;
2162
2163 // Cleanup of old prim geometry and Bodies
2164 if (IsPhysical && Body != IntPtr.Zero)
2165 {
2166 if (childPrim)
2167 {
2168 if (_parent != null)
2169 {
2170 OdePrim parent = (OdePrim)_parent;
2171 parent.ChildDelink(this);
2172 }
2173 }
2174 else
2175 {
2176 disableBody();
2177 }
2178 }
2179
2180 RemoveGeom();
2181
2182 // we don't need to do space calculation because the client sends a position update also.
2183 if (_size.X <= 0) _size.X = 0.01f;
2184 if (_size.Y <= 0) _size.Y = 0.01f;
2185 if (_size.Z <= 0) _size.Z = 0.01f;
2186 // Construction of new prim
2187
2188 IMesh mesh = null;
2189
2190
2191 if (_parent_scene.needsMeshing(_pbs))
2192 {
2193 // Don't need to re-enable body.. it's done in CreateMesh
2194 float meshlod = _parent_scene.meshSculptLOD;
2195
2196 if (IsPhysical)
2197 meshlod = _parent_scene.MeshSculptphysicalLOD;
2198
2199 // createmesh returns null when it doesn't mesh.
2200 mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical);
2201 if (mesh == null)
2202 CheckMeshAsset();
2203 else
2204 m_assetFailed = false;
2205 }
2206
2207 CreateGeom(m_targetSpace, mesh);
2208 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2209 d.Quaternion myrot = new d.Quaternion();
2210 //myrot.W = _orientation.w;
2211 myrot.W = _orientation.W;
2212 myrot.X = _orientation.X;
2213 myrot.Y = _orientation.Y;
2214 myrot.Z = _orientation.Z;
2215 d.GeomSetQuaternion(prim_geom, ref myrot);
2216
2217 //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
2218 if (IsPhysical && Body == IntPtr.Zero)
2219 {
2220 // Re creates body on size.
2221 // EnableBody also does setMass()
2222 enableBody();
2223 if (Body != IntPtr.Zero)
2224 {
2225 d.BodyEnable(Body);
2226 }
2227 }
2228
2229 changeSelectedStatus();
2230
2231 if (childPrim)
2232 {
2233 if (_parent is OdePrim)
2234 {
2235 OdePrim parent = (OdePrim)_parent;
2236 parent.ChildSetGeom(this);
2237 }
2238 }
2239
2240 resetCollisionAccounting();
2241// m_taintshape = false;
2242 }
2243
2244 /// <summary>
2245 /// Change prim in response to an add force taint.
2246 /// </summary>
2247 private void changeAddForce()
2248 {
2249 if (!m_isSelected)
2250 {
2251 lock (m_forcelist)
2252 {
2253 //m_log.Info("[PHYSICS]: dequeing forcelist");
2254 if (IsPhysical)
2255 {
2256 Vector3 iforce = Vector3.Zero;
2257 int i = 0;
2258 try
2259 {
2260 for (i = 0; i < m_forcelist.Count; i++)
2261 {
2262
2263 iforce = iforce + (m_forcelist[i] * 100);
2264 }
2265 }
2266 catch (IndexOutOfRangeException)
2267 {
2268 m_forcelist = new List<Vector3>();
2269 m_collisionscore = 0;
2270 m_interpenetrationcount = 0;
2271 m_taintforce = false;
2272 return;
2273 }
2274 catch (ArgumentOutOfRangeException)
2275 {
2276 m_forcelist = new List<Vector3>();
2277 m_collisionscore = 0;
2278 m_interpenetrationcount = 0;
2279 m_taintforce = false;
2280 return;
2281 }
2282 d.BodyEnable(Body);
2283 d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z);
2284 }
2285 m_forcelist.Clear();
2286 }
2287
2288 m_collisionscore = 0;
2289 m_interpenetrationcount = 0;
2290 }
2291
2292 m_taintforce = false;
2293 }
2294
2295 /// <summary>
2296 /// Change prim in response to a torque taint.
2297 /// </summary>
2298 private void changeSetTorque()
2299 {
2300 if (!m_isSelected)
2301 {
2302 if (IsPhysical && Body != IntPtr.Zero)
2303 {
2304 d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z);
2305 }
2306 }
2307
2308 m_taintTorque = Vector3.Zero;
2309 }
2310
2311 /// <summary>
2312 /// Change prim in response to an angular force taint.
2313 /// </summary>
2314 private void changeAddAngularForce()
2315 {
2316 if (!m_isSelected)
2317 {
2318 lock (m_angularforcelist)
2319 {
2320 //m_log.Info("[PHYSICS]: dequeing forcelist");
2321 if (IsPhysical)
2322 {
2323 Vector3 iforce = Vector3.Zero;
2324 for (int i = 0; i < m_angularforcelist.Count; i++)
2325 {
2326 iforce = iforce + (m_angularforcelist[i] * 100);
2327 }
2328 d.BodyEnable(Body);
2329 d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z);
2330
2331 }
2332 m_angularforcelist.Clear();
2333 }
2334
2335 m_collisionscore = 0;
2336 m_interpenetrationcount = 0;
2337 }
2338
2339 m_taintaddangularforce = false;
2340 }
2341
2342 /// <summary>
2343 /// Change prim in response to a velocity taint.
2344 /// </summary>
2345 private void changevelocity()
2346 {
2347 if (!m_isSelected)
2348 {
2349 // Not sure exactly why this sleep is here, but from experimentation it appears to stop an avatar
2350 // walking through a default rez size prim if it keeps kicking it around - justincc.
2351 Thread.Sleep(20);
2352
2353 if (IsPhysical)
2354 {
2355 if (Body != IntPtr.Zero)
2356 {
2357 d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
2358 }
2359 }
2360
2361 //resetCollisionAccounting();
2362 }
2363
2364 m_taintVelocity = Vector3.Zero;
2365 }
2366
2367 internal void setPrimForRemoval()
2368 {
2369 m_taintremove = true;
2370 }
2371
2372 public override bool Flying
2373 {
2374 // no flying prims for you
2375 get { return false; }
2376 set { }
2377 }
2378
2379 public override bool IsColliding
2380 {
2381 get { return iscolliding; }
2382 set { iscolliding = value; }
2383 }
2384
2385 public override bool CollidingGround
2386 {
2387 get { return false; }
2388 set { return; }
2389 }
2390
2391 public override bool CollidingObj
2392 {
2393 get { return false; }
2394 set { return; }
2395 }
2396
2397 public override bool ThrottleUpdates
2398 {
2399 get { return m_throttleUpdates; }
2400 set { m_throttleUpdates = value; }
2401 }
2402
2403 public override bool Stopped
2404 {
2405 get { return _zeroFlag; }
2406 }
2407
2408 public override Vector3 Position
2409 {
2410 get { return _position; }
2411
2412 set { _position = value;
2413 //m_log.Info("[PHYSICS]: " + _position.ToString());
2414 }
2415 }
2416
2417 public override Vector3 Size
2418 {
2419 get { return _size; }
2420 set
2421 {
2422 if (value.IsFinite())
2423 {
2424 _size = value;
2425// m_log.DebugFormat("[PHYSICS]: Set size on {0} to {1}", Name, value);
2426 }
2427 else
2428 {
2429 m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name);
2430 }
2431 }
2432 }
2433
2434 public override float Mass
2435 {
2436 get { return CalculateMass(); }
2437 }
2438
2439 public override Vector3 Force
2440 {
2441 //get { return Vector3.Zero; }
2442 get { return m_force; }
2443 set
2444 {
2445 if (value.IsFinite())
2446 {
2447 m_force = value;
2448 }
2449 else
2450 {
2451 m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name);
2452 }
2453 }
2454 }
2455
2456 public override int VehicleType
2457 {
2458 get { return (int)m_vehicle.Type; }
2459 set { m_vehicle.ProcessTypeChange((Vehicle)value); }
2460 }
2461
2462 public override void VehicleFloatParam(int param, float value)
2463 {
2464 m_vehicle.ProcessFloatVehicleParam((Vehicle) param, value);
2465 }
2466
2467 public override void VehicleVectorParam(int param, Vector3 value)
2468 {
2469 m_vehicle.ProcessVectorVehicleParam((Vehicle) param, value);
2470 }
2471
2472 public override void VehicleRotationParam(int param, Quaternion rotation)
2473 {
2474 m_vehicle.ProcessRotationVehicleParam((Vehicle) param, rotation);
2475 }
2476
2477 public override void VehicleFlags(int param, bool remove)
2478 {
2479 m_vehicle.ProcessVehicleFlags(param, remove);
2480 }
2481
2482 public override void SetVolumeDetect(int param)
2483 {
2484 // We have to lock the scene here so that an entire simulate loop either uses volume detect for all
2485 // possible collisions with this prim or for none of them.
2486 lock (_parent_scene.OdeLock)
2487 {
2488 m_isVolumeDetect = (param != 0);
2489 }
2490 }
2491
2492 public override Vector3 CenterOfMass
2493 {
2494 get { return Vector3.Zero; }
2495 }
2496
2497 public override Vector3 GeometricCenter
2498 {
2499 get { return Vector3.Zero; }
2500 }
2501
2502 public override PrimitiveBaseShape Shape
2503 {
2504 set
2505 {
2506 _pbs = value;
2507 m_assetFailed = false;
2508 m_taintshape = true;
2509 }
2510 }
2511
2512 public override Vector3 Velocity
2513 {
2514 get
2515 {
2516 // Average previous velocity with the new one so
2517 // client object interpolation works a 'little' better
2518 if (_zeroFlag)
2519 return Vector3.Zero;
2520
2521 Vector3 returnVelocity = Vector3.Zero;
2522 returnVelocity.X = (m_lastVelocity.X + _velocity.X) * 0.5f; // 0.5f is mathematically equiv to '/ 2'
2523 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) * 0.5f;
2524 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) * 0.5f;
2525 return returnVelocity;
2526 }
2527 set
2528 {
2529 if (value.IsFinite())
2530 {
2531 _velocity = value;
2532
2533 m_taintVelocity = value;
2534 _parent_scene.AddPhysicsActorTaint(this);
2535 }
2536 else
2537 {
2538 m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name);
2539 }
2540
2541 }
2542 }
2543
2544 public override Vector3 Torque
2545 {
2546 get
2547 {
2548 if (!IsPhysical || Body == IntPtr.Zero)
2549 return Vector3.Zero;
2550
2551 return _torque;
2552 }
2553
2554 set
2555 {
2556 if (value.IsFinite())
2557 {
2558 m_taintTorque = value;
2559 _parent_scene.AddPhysicsActorTaint(this);
2560 }
2561 else
2562 {
2563 m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name);
2564 }
2565 }
2566 }
2567
2568 public override float CollisionScore
2569 {
2570 get { return m_collisionscore; }
2571 set { m_collisionscore = value; }
2572 }
2573
2574 public override bool Kinematic
2575 {
2576 get { return false; }
2577 set { }
2578 }
2579
2580 public override Quaternion Orientation
2581 {
2582 get { return _orientation; }
2583 set
2584 {
2585 if (QuaternionIsFinite(value))
2586 _orientation = value;
2587 else
2588 m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name);
2589 }
2590 }
2591
2592 private static bool QuaternionIsFinite(Quaternion q)
2593 {
2594 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
2595 return false;
2596 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
2597 return false;
2598 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
2599 return false;
2600 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
2601 return false;
2602 return true;
2603 }
2604
2605 public override Vector3 Acceleration
2606 {
2607 get { return _acceleration; }
2608 set { _acceleration = value; }
2609 }
2610
2611 public override void AddForce(Vector3 force, bool pushforce)
2612 {
2613 if (force.IsFinite())
2614 {
2615 lock (m_forcelist)
2616 m_forcelist.Add(force);
2617
2618 m_taintforce = true;
2619 }
2620 else
2621 {
2622 m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name);
2623 }
2624 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
2625 }
2626
2627 public override void AddAngularForce(Vector3 force, bool pushforce)
2628 {
2629 if (force.IsFinite())
2630 {
2631 m_angularforcelist.Add(force);
2632 m_taintaddangularforce = true;
2633 }
2634 else
2635 {
2636 m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name);
2637 }
2638 }
2639
2640 public override Vector3 RotationalVelocity
2641 {
2642 get
2643 {
2644 Vector3 pv = Vector3.Zero;
2645 if (_zeroFlag)
2646 return pv;
2647 m_lastUpdateSent = false;
2648
2649 if (m_rotationalVelocity.ApproxEquals(pv, 0.2f))
2650 return pv;
2651
2652 return m_rotationalVelocity;
2653 }
2654 set
2655 {
2656 if (value.IsFinite())
2657 {
2658 m_rotationalVelocity = value;
2659 setAngularVelocity(value.X, value.Y, value.Z);
2660 }
2661 else
2662 {
2663 m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name);
2664 }
2665 }
2666 }
2667
2668 public override void CrossingFailure()
2669 {
2670 /*
2671 m_crossingfailures++;
2672 if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
2673 {
2674 base.RaiseOutOfBounds(_position);
2675 return;
2676 }
2677 else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
2678 {
2679 m_log.Warn("[PHYSICS]: Too many crossing failures for: " + Name);
2680 }
2681 */
2682 _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);
2683 _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);
2684 _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f);
2685
2686 m_lastposition = _position;
2687 _velocity.X = 0;
2688 _velocity.Y = 0;
2689 _velocity.Z = 0;
2690
2691 m_lastVelocity = _velocity;
2692
2693 if (Body != IntPtr.Zero)
2694 {
2695 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
2696 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
2697 }
2698
2699 outofBounds = false;
2700 base.RequestPhysicsterseUpdate();
2701
2702 }
2703
2704 public override float Buoyancy
2705 {
2706 get { return m_buoyancy; }
2707 set { m_buoyancy = value; }
2708 }
2709
2710 public override void link(PhysicsActor obj)
2711 {
2712 m_taintparent = obj;
2713 }
2714
2715 public override void delink()
2716 {
2717 m_taintparent = null;
2718 }
2719
2720 public override void LockAngularMotion(Vector3 axis)
2721 {
2722 // reverse the zero/non zero values for ODE.
2723 if (axis.IsFinite())
2724 {
2725 axis.X = (axis.X > 0) ? 1f : 0f;
2726 axis.Y = (axis.Y > 0) ? 1f : 0f;
2727 axis.Z = (axis.Z > 0) ? 1f : 0f;
2728 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
2729 m_taintAngularLock = axis;
2730 }
2731 else
2732 {
2733 m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name);
2734 }
2735 }
2736
2737 internal void UpdatePositionAndVelocity()
2738 {
2739 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
2740 if (outofBounds)
2741 return;
2742 if (_parent == null)
2743 {
2744 Vector3 pv = Vector3.Zero;
2745 bool lastZeroFlag = _zeroFlag;
2746 float m_minvelocity = 0;
2747 if (Body != (IntPtr)0) // FIXME -> or if it is a joint
2748 {
2749 d.Vector3 vec = d.BodyGetPosition(Body);
2750 d.Quaternion ori = d.BodyGetQuaternion(Body);
2751 d.Vector3 vel = d.BodyGetLinearVel(Body);
2752 d.Vector3 rotvel = d.BodyGetAngularVel(Body);
2753 d.Vector3 torque = d.BodyGetTorque(Body);
2754 _torque = new Vector3(torque.X, torque.Y, torque.Z);
2755 Vector3 l_position = Vector3.Zero;
2756 Quaternion l_orientation = Quaternion.Identity;
2757
2758 m_lastposition = _position;
2759 m_lastorientation = _orientation;
2760
2761 l_position.X = vec.X;
2762 l_position.Y = vec.Y;
2763 l_position.Z = vec.Z;
2764 l_orientation.X = ori.X;
2765 l_orientation.Y = ori.Y;
2766 l_orientation.Z = ori.Z;
2767 l_orientation.W = ori.W;
2768
2769 if (l_position.Z < 0)
2770 {
2771 // This is so prim that get lost underground don't fall forever and suck up
2772 //
2773 // Sim resources and memory.
2774 // Disables the prim's movement physics....
2775 // It's a hack and will generate a console message if it fails.
2776
2777 //IsPhysical = false;
2778
2779 _acceleration.X = 0;
2780 _acceleration.Y = 0;
2781 _acceleration.Z = 0;
2782
2783 _velocity.X = 0;
2784 _velocity.Y = 0;
2785 _velocity.Z = 0;
2786 m_rotationalVelocity.X = 0;
2787 m_rotationalVelocity.Y = 0;
2788 m_rotationalVelocity.Z = 0;
2789
2790 if (_parent == null)
2791 base.RaiseOutOfBounds(_position);
2792
2793 if (_parent == null)
2794 base.RequestPhysicsterseUpdate();
2795
2796 m_throttleUpdates = false;
2797 throttleCounter = 0;
2798 _zeroFlag = true;
2799 //outofBounds = true;
2800 return;
2801 }
2802
2803 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)
2804 {
2805 //base.RaiseOutOfBounds(l_position);
2806 /*
2807 if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
2808 {
2809 _position = l_position;
2810 //_parent_scene.remActivePrim(this);
2811 if (_parent == null)
2812 base.RequestPhysicsterseUpdate();
2813 return;
2814 }
2815 else
2816 {
2817 if (_parent == null)
2818 base.RaiseOutOfBounds(l_position);
2819 return;
2820 }
2821 */
2822 outofBounds = true;
2823 // part near the border on outside
2824 if (l_position.X < 0)
2825 Util.Clamp(l_position.X, -0.1f, -2f);
2826 else
2827 Util.Clamp(l_position.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f);
2828 if (l_position.Y < 0)
2829 Util.Clamp(l_position.Y, -0.1f, -2f);
2830 else
2831 Util.Clamp(l_position.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f);
2832
2833 d.BodySetPosition(Body, l_position.X, l_position.Y, l_position.Z);
2834
2835 // stop it
2836 d.BodySetAngularVel(Body, 0, 0, 0);
2837 d.BodySetLinearVel(Body, 0, 0, 0);
2838 disableBodySoft();
2839
2840 // tell framework to fix it
2841 if (_parent == null)
2842 base.RequestPhysicsterseUpdate();
2843 return;
2844 }
2845
2846
2847 //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation));
2848 //Console.WriteLine("Adiff " + Name + " = " + Adiff);
2849 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
2850 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
2851 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
2852// && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01))
2853 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large
2854 {
2855 _zeroFlag = true;
2856//Console.WriteLine("ZFT 2");
2857 m_throttleUpdates = false;
2858 }
2859 else
2860 {
2861 //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
2862 _zeroFlag = false;
2863 m_lastUpdateSent = false;
2864 //m_throttleUpdates = false;
2865 }
2866
2867 if (_zeroFlag)
2868 {
2869 _velocity.X = 0.0f;
2870 _velocity.Y = 0.0f;
2871 _velocity.Z = 0.0f;
2872
2873 _acceleration.X = 0;
2874 _acceleration.Y = 0;
2875 _acceleration.Z = 0;
2876
2877 //_orientation.w = 0f;
2878 //_orientation.X = 0f;
2879 //_orientation.Y = 0f;
2880 //_orientation.Z = 0f;
2881 m_rotationalVelocity.X = 0;
2882 m_rotationalVelocity.Y = 0;
2883 m_rotationalVelocity.Z = 0;
2884 if (!m_lastUpdateSent)
2885 {
2886 m_throttleUpdates = false;
2887 throttleCounter = 0;
2888 m_rotationalVelocity = pv;
2889
2890 if (_parent == null)
2891 {
2892 base.RequestPhysicsterseUpdate();
2893 }
2894
2895 m_lastUpdateSent = true;
2896 }
2897 }
2898 else
2899 {
2900 if (lastZeroFlag != _zeroFlag)
2901 {
2902 if (_parent == null)
2903 {
2904 base.RequestPhysicsterseUpdate();
2905 }
2906 }
2907
2908 m_lastVelocity = _velocity;
2909
2910 _position = l_position;
2911
2912 _velocity.X = vel.X;
2913 _velocity.Y = vel.Y;
2914 _velocity.Z = vel.Z;
2915
2916 _acceleration = ((_velocity - m_lastVelocity) / 0.1f);
2917 _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f);
2918 //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());
2919
2920 // Note here that linearvelocity is affecting angular velocity... so I'm guessing this is a vehicle specific thing...
2921 // it does make sense to do this for tiny little instabilities with physical prim, however 0.5m/frame is fairly large.
2922 // 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
2923 // adding these logical exclusion situations to maintain this where I think it was intended to be.
2924 if (m_throttleUpdates || PIDActive || (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) || (Amotor != IntPtr.Zero))
2925 {
2926 m_minvelocity = 0.5f;
2927 }
2928 else
2929 {
2930 m_minvelocity = 0.02f;
2931 }
2932
2933 if (_velocity.ApproxEquals(pv, m_minvelocity))
2934 {
2935 m_rotationalVelocity = pv;
2936 }
2937 else
2938 {
2939 m_rotationalVelocity = new Vector3(rotvel.X, rotvel.Y, rotvel.Z);
2940 }
2941
2942 //m_log.Debug("ODE: " + m_rotationalVelocity.ToString());
2943 _orientation.X = ori.X;
2944 _orientation.Y = ori.Y;
2945 _orientation.Z = ori.Z;
2946 _orientation.W = ori.W;
2947 m_lastUpdateSent = false;
2948 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
2949 {
2950 if (_parent == null)
2951 {
2952 base.RequestPhysicsterseUpdate();
2953 }
2954 }
2955 else
2956 {
2957 throttleCounter++;
2958 }
2959 }
2960 m_lastposition = l_position;
2961 }
2962 else
2963 {
2964 // Not a body.. so Make sure the client isn't interpolating
2965 _velocity.X = 0;
2966 _velocity.Y = 0;
2967 _velocity.Z = 0;
2968
2969 _acceleration.X = 0;
2970 _acceleration.Y = 0;
2971 _acceleration.Z = 0;
2972
2973 m_rotationalVelocity.X = 0;
2974 m_rotationalVelocity.Y = 0;
2975 m_rotationalVelocity.Z = 0;
2976 _zeroFlag = true;
2977 }
2978 }
2979 }
2980
2981 public override bool FloatOnWater
2982 {
2983 set {
2984 m_taintCollidesWater = value;
2985 _parent_scene.AddPhysicsActorTaint(this);
2986 }
2987 }
2988
2989 public override void SetMomentum(Vector3 momentum)
2990 {
2991 }
2992
2993 public override Vector3 PIDTarget
2994 {
2995 set
2996 {
2997 if (value.IsFinite())
2998 {
2999 m_PIDTarget = value;
3000 }
3001 else
3002 m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name);
3003 }
3004 }
3005
3006 public override bool PIDActive { get; set; }
3007 public override float PIDTau { set { m_PIDTau = value; } }
3008
3009 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
3010 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
3011 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
3012 public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
3013
3014 public override Quaternion APIDTarget{ set { return; } }
3015
3016 public override bool APIDActive{ set { return; } }
3017
3018 public override float APIDStrength{ set { return; } }
3019
3020 public override float APIDDamping{ set { return; } }
3021
3022 private void createAMotor(Vector3 axis)
3023 {
3024 if (Body == IntPtr.Zero)
3025 return;
3026
3027 if (Amotor != IntPtr.Zero)
3028 {
3029 d.JointDestroy(Amotor);
3030 Amotor = IntPtr.Zero;
3031 }
3032
3033 float axisnum = 3;
3034
3035 axisnum = (axisnum - (axis.X + axis.Y + axis.Z));
3036
3037 // PhysicsVector totalSize = new PhysicsVector(_size.X, _size.Y, _size.Z);
3038
3039
3040 // Inverse Inertia Matrix, set the X, Y, and/r Z inertia to 0 then invert it again.
3041 d.Mass objMass;
3042 d.MassSetZero(out objMass);
3043 DMassCopy(ref pMass, ref objMass);
3044
3045 //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);
3046
3047 Matrix4 dMassMat = FromDMass(objMass);
3048
3049 Matrix4 mathmat = Inverse(dMassMat);
3050
3051 /*
3052 //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]);
3053
3054 mathmat = Inverse(mathmat);
3055
3056
3057 objMass = FromMatrix4(mathmat, ref objMass);
3058 //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);
3059
3060 mathmat = Inverse(mathmat);
3061 */
3062 if (axis.X == 0)
3063 {
3064 mathmat.M33 = 50.0000001f;
3065 //objMass.I.M22 = 0;
3066 }
3067 if (axis.Y == 0)
3068 {
3069 mathmat.M22 = 50.0000001f;
3070 //objMass.I.M11 = 0;
3071 }
3072 if (axis.Z == 0)
3073 {
3074 mathmat.M11 = 50.0000001f;
3075 //objMass.I.M00 = 0;
3076 }
3077
3078
3079
3080 mathmat = Inverse(mathmat);
3081 objMass = FromMatrix4(mathmat, ref objMass);
3082 //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);
3083
3084 //return;
3085 if (d.MassCheck(ref objMass))
3086 {
3087 d.BodySetMass(Body, ref objMass);
3088 }
3089 else
3090 {
3091 //m_log.Debug("[PHYSICS]: Mass invalid, ignoring");
3092 }
3093
3094 if (axisnum <= 0)
3095 return;
3096 // int dAMotorEuler = 1;
3097
3098 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
3099 d.JointAttach(Amotor, Body, IntPtr.Zero);
3100 d.JointSetAMotorMode(Amotor, 0);
3101
3102 d.JointSetAMotorNumAxes(Amotor,(int)axisnum);
3103 int i = 0;
3104
3105 if (axis.X == 0)
3106 {
3107 d.JointSetAMotorAxis(Amotor, i, 0, 1, 0, 0);
3108 i++;
3109 }
3110
3111 if (axis.Y == 0)
3112 {
3113 d.JointSetAMotorAxis(Amotor, i, 0, 0, 1, 0);
3114 i++;
3115 }
3116
3117 if (axis.Z == 0)
3118 {
3119 d.JointSetAMotorAxis(Amotor, i, 0, 0, 0, 1);
3120 i++;
3121 }
3122
3123 for (int j = 0; j < (int)axisnum; j++)
3124 {
3125 //d.JointSetAMotorAngle(Amotor, j, 0);
3126 }
3127
3128 //d.JointSetAMotorAngle(Amotor, 1, 0);
3129 //d.JointSetAMotorAngle(Amotor, 2, 0);
3130
3131 // These lowstops and high stops are effectively (no wiggle room)
3132 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0f);
3133 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f);
3134 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0f);
3135 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
3136 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
3137 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
3138 //d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 9000f);
3139 d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
3140 d.JointSetAMotorParam(Amotor, (int)dParam.FMax, Mass * 50f);//
3141 }
3142
3143 private Matrix4 FromDMass(d.Mass pMass)
3144 {
3145 Matrix4 obj;
3146 obj.M11 = pMass.I.M00;
3147 obj.M12 = pMass.I.M01;
3148 obj.M13 = pMass.I.M02;
3149 obj.M14 = 0;
3150 obj.M21 = pMass.I.M10;
3151 obj.M22 = pMass.I.M11;
3152 obj.M23 = pMass.I.M12;
3153 obj.M24 = 0;
3154 obj.M31 = pMass.I.M20;
3155 obj.M32 = pMass.I.M21;
3156 obj.M33 = pMass.I.M22;
3157 obj.M34 = 0;
3158 obj.M41 = 0;
3159 obj.M42 = 0;
3160 obj.M43 = 0;
3161 obj.M44 = 1;
3162 return obj;
3163 }
3164
3165 private d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
3166 {
3167 obj.I.M00 = pMat[0, 0];
3168 obj.I.M01 = pMat[0, 1];
3169 obj.I.M02 = pMat[0, 2];
3170 obj.I.M10 = pMat[1, 0];
3171 obj.I.M11 = pMat[1, 1];
3172 obj.I.M12 = pMat[1, 2];
3173 obj.I.M20 = pMat[2, 0];
3174 obj.I.M21 = pMat[2, 1];
3175 obj.I.M22 = pMat[2, 2];
3176 return obj;
3177 }
3178
3179 public override void SubscribeEvents(int ms)
3180 {
3181 m_eventsubscription = ms;
3182 _parent_scene.AddCollisionEventReporting(this);
3183 }
3184
3185 public override void UnSubscribeEvents()
3186 {
3187 _parent_scene.RemoveCollisionEventReporting(this);
3188 m_eventsubscription = 0;
3189 }
3190
3191 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
3192 {
3193 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
3194 }
3195
3196 public void SendCollisions()
3197 {
3198 if (m_collisionsOnPreviousFrame || CollisionEventsThisFrame.Count > 0)
3199 {
3200 base.SendCollisionUpdate(CollisionEventsThisFrame);
3201
3202 if (CollisionEventsThisFrame.Count > 0)
3203 {
3204 m_collisionsOnPreviousFrame = true;
3205 CollisionEventsThisFrame.Clear();
3206 }
3207 else
3208 {
3209 m_collisionsOnPreviousFrame = false;
3210 }
3211 }
3212 }
3213
3214 public override bool SubscribedEvents()
3215 {
3216 if (m_eventsubscription > 0)
3217 return true;
3218 return false;
3219 }
3220
3221 public static Matrix4 Inverse(Matrix4 pMat)
3222 {
3223 if (determinant3x3(pMat) == 0)
3224 {
3225 return Matrix4.Identity; // should probably throw an error. singluar matrix inverse not possible
3226 }
3227
3228 return (Adjoint(pMat) / determinant3x3(pMat));
3229 }
3230
3231 public static Matrix4 Adjoint(Matrix4 pMat)
3232 {
3233 Matrix4 adjointMatrix = new Matrix4();
3234 for (int i=0; i<4; i++)
3235 {
3236 for (int j=0; j<4; j++)
3237 {
3238 Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j)))));
3239 }
3240 }
3241
3242 adjointMatrix = Transpose(adjointMatrix);
3243 return adjointMatrix;
3244 }
3245
3246 public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol)
3247 {
3248 Matrix4 minor = new Matrix4();
3249 int m = 0, n = 0;
3250 for (int i = 0; i < 4; i++)
3251 {
3252 if (i == iRow)
3253 continue;
3254 n = 0;
3255 for (int j = 0; j < 4; j++)
3256 {
3257 if (j == iCol)
3258 continue;
3259 Matrix4SetValue(ref minor, m,n, matrix[i, j]);
3260 n++;
3261 }
3262 m++;
3263 }
3264
3265 return minor;
3266 }
3267
3268 public static Matrix4 Transpose(Matrix4 pMat)
3269 {
3270 Matrix4 transposeMatrix = new Matrix4();
3271 for (int i = 0; i < 4; i++)
3272 for (int j = 0; j < 4; j++)
3273 Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]);
3274 return transposeMatrix;
3275 }
3276
3277 public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val)
3278 {
3279 switch (r)
3280 {
3281 case 0:
3282 switch (c)
3283 {
3284 case 0:
3285 pMat.M11 = val;
3286 break;
3287 case 1:
3288 pMat.M12 = val;
3289 break;
3290 case 2:
3291 pMat.M13 = val;
3292 break;
3293 case 3:
3294 pMat.M14 = val;
3295 break;
3296 }
3297
3298 break;
3299 case 1:
3300 switch (c)
3301 {
3302 case 0:
3303 pMat.M21 = val;
3304 break;
3305 case 1:
3306 pMat.M22 = val;
3307 break;
3308 case 2:
3309 pMat.M23 = val;
3310 break;
3311 case 3:
3312 pMat.M24 = val;
3313 break;
3314 }
3315
3316 break;
3317 case 2:
3318 switch (c)
3319 {
3320 case 0:
3321 pMat.M31 = val;
3322 break;
3323 case 1:
3324 pMat.M32 = val;
3325 break;
3326 case 2:
3327 pMat.M33 = val;
3328 break;
3329 case 3:
3330 pMat.M34 = val;
3331 break;
3332 }
3333
3334 break;
3335 case 3:
3336 switch (c)
3337 {
3338 case 0:
3339 pMat.M41 = val;
3340 break;
3341 case 1:
3342 pMat.M42 = val;
3343 break;
3344 case 2:
3345 pMat.M43 = val;
3346 break;
3347 case 3:
3348 pMat.M44 = val;
3349 break;
3350 }
3351
3352 break;
3353 }
3354 }
3355
3356 private static float determinant3x3(Matrix4 pMat)
3357 {
3358 float det = 0;
3359 float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2];
3360 float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0];
3361 float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1];
3362 float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2];
3363 float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0];
3364 float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1];
3365
3366 det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6);
3367 return det;
3368 }
3369
3370 private static void DMassCopy(ref d.Mass src, ref d.Mass dst)
3371 {
3372 dst.c.W = src.c.W;
3373 dst.c.X = src.c.X;
3374 dst.c.Y = src.c.Y;
3375 dst.c.Z = src.c.Z;
3376 dst.mass = src.mass;
3377 dst.I.M00 = src.I.M00;
3378 dst.I.M01 = src.I.M01;
3379 dst.I.M02 = src.I.M02;
3380 dst.I.M10 = src.I.M10;
3381 dst.I.M11 = src.I.M11;
3382 dst.I.M12 = src.I.M12;
3383 dst.I.M20 = src.I.M20;
3384 dst.I.M21 = src.I.M21;
3385 dst.I.M22 = src.I.M22;
3386 }
3387
3388 public override void SetMaterial(int pMaterial)
3389 {
3390 m_material = pMaterial;
3391 }
3392
3393 private void CheckMeshAsset()
3394 {
3395 if (_pbs.SculptEntry && !m_assetFailed && _pbs.SculptTexture != UUID.Zero)
3396 {
3397 m_assetFailed = true;
3398 Util.FireAndForget(delegate
3399 {
3400 RequestAssetDelegate assetProvider = _parent_scene.RequestAssetMethod;
3401 if (assetProvider != null)
3402 assetProvider(_pbs.SculptTexture, MeshAssetReceived);
3403 }, null, "ODEPrim.CheckMeshAsset");
3404 }
3405 }
3406
3407 private void MeshAssetReceived(AssetBase asset)
3408 {
3409 if (asset != null && asset.Data != null && asset.Data.Length > 0)
3410 {
3411 if (!_pbs.SculptEntry)
3412 return;
3413 if (_pbs.SculptTexture.ToString() != asset.ID)
3414 return;
3415
3416 _pbs.SculptData = new byte[asset.Data.Length];
3417 asset.Data.CopyTo(_pbs.SculptData, 0);
3418// m_assetFailed = false;
3419
3420// m_log.DebugFormat(
3421// "[ODE PRIM]: Received mesh/sculpt data asset {0} with {1} bytes for {2} at {3} in {4}",
3422// _pbs.SculptTexture, _pbs.SculptData.Length, Name, _position, _parent_scene.Name);
3423
3424 m_taintshape = true;
3425 _parent_scene.AddPhysicsActorTaint(this);
3426 }
3427 else
3428 {
3429 m_log.WarnFormat(
3430 "[ODE PRIM]: Could not get mesh/sculpt asset {0} for {1} at {2} in {3}",
3431 _pbs.SculptTexture, Name, _position, _parent_scene.PhysicsSceneName);
3432 }
3433 }
3434 }
3435} \ No newline at end of file
generated by cgit v1.1 at 2024-09-12 17:38:02 +0000