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