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