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