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