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