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