aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs')
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs2584
1 files changed, 1635 insertions, 949 deletions
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
index e3ac315..e51ef5b 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -22,32 +22,10 @@
22 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
23 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
24 * 24 *
25 * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces 25 * Revised March 5th 2010 by Kitto Flora. ODEDynamics.cs
26 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: 26 * rolled into ODEPrim.cs
27 * ODEPrim.cs contains methods dealing with Prim editing, Prim
28 * characteristics and Kinetic motion.
29 * ODEDynamics.cs contains methods dealing with Prim Physical motion
30 * (dynamics) and the associated settings. Old Linear and angular
31 * motors for dynamic motion have been replace with MoveLinear()
32 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
33 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
34 * switch between 'VEHICLE' parameter use and general dynamics
35 * settings use.
36 */ 27 */
37 28
38/*
39 * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces
40 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
41 * ODEPrim.cs contains methods dealing with Prim editing, Prim
42 * characteristics and Kinetic motion.
43 * ODEDynamics.cs contains methods dealing with Prim Physical motion
44 * (dynamics) and the associated settings. Old Linear and angular
45 * motors for dynamic motion have been replace with MoveLinear()
46 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
47 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
48 * switch between 'VEHICLE' parameter use and general dynamics
49 * settings use.
50 */
51using System; 29using System;
52using System.Collections.Generic; 30using System.Collections.Generic;
53using System.Reflection; 31using System.Reflection;
@@ -59,6 +37,7 @@ using Ode.NET;
59using OpenSim.Framework; 37using OpenSim.Framework;
60using OpenSim.Region.Physics.Manager; 38using OpenSim.Region.Physics.Manager;
61 39
40
62namespace OpenSim.Region.Physics.OdePlugin 41namespace OpenSim.Region.Physics.OdePlugin
63{ 42{
64 /// <summary> 43 /// <summary>
@@ -102,8 +81,8 @@ namespace OpenSim.Region.Physics.OdePlugin
102 private float m_APIDDamping = 0.5f; 81 private float m_APIDDamping = 0.5f;
103 private bool m_useAPID = false; 82 private bool m_useAPID = false;
104 83
105 // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), 84 // These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
106 // and are for non-VEHICLES only. 85 // do not confuse with VEHICLE HOVER
107 86
108 private float m_PIDHoverHeight; 87 private float m_PIDHoverHeight;
109 private float m_PIDHoverTau; 88 private float m_PIDHoverTau;
@@ -112,7 +91,7 @@ namespace OpenSim.Region.Physics.OdePlugin
112 private float m_targetHoverHeight; 91 private float m_targetHoverHeight;
113 private float m_groundHeight; 92 private float m_groundHeight;
114 private float m_waterHeight; 93 private float m_waterHeight;
115 private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. 94 private float m_buoyancy; //m_buoyancy set by llSetBuoyancy()
116 95
117 // private float m_tensor = 5f; 96 // private float m_tensor = 5f;
118 private int body_autodisable_frames = 20; 97 private int body_autodisable_frames = 20;
@@ -183,7 +162,7 @@ namespace OpenSim.Region.Physics.OdePlugin
183 public bool outofBounds; 162 public bool outofBounds;
184 private float m_density = 10.000006836f; // Aluminum g/cm3; 163 private float m_density = 10.000006836f; // Aluminum g/cm3;
185 164
186 public bool _zeroFlag; 165 public bool _zeroFlag; // if body has been stopped
187 private bool m_lastUpdateSent; 166 private bool m_lastUpdateSent;
188 167
189 public IntPtr Body = IntPtr.Zero; 168 public IntPtr Body = IntPtr.Zero;
@@ -198,18 +177,81 @@ namespace OpenSim.Region.Physics.OdePlugin
198 177
199 public volatile bool childPrim; 178 public volatile bool childPrim;
200 179
201 private ODEDynamics m_vehicle;
202
203 internal int m_material = (int)Material.Wood; 180 internal int m_material = (int)Material.Wood;
204 181
205 private int frcount = 0; // Used to limit dynamics debug output to 182 private int frcount = 0; // Used to limit dynamics debug output to
206 183
184 private IntPtr m_body = IntPtr.Zero;
185
186 // Vehicle properties ============================================================================================
187 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
188 // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
189 private VehicleFlag m_flags = (VehicleFlag) 0; // Bit settings:
190 // HOVER_TERRAIN_ONLY
191 // HOVER_GLOBAL_HEIGHT
192 // NO_DEFLECTION_UP
193 // HOVER_WATER_ONLY
194 // HOVER_UP_ONLY
195 // LIMIT_MOTOR_UP
196 // LIMIT_ROLL_ONLY
197
198 // Linear properties
199 private Vector3 m_linearMotorDirection = Vector3.Zero; // (was m_linearMotorDirectionLASTSET) the (local) Velocity
200 //requested by LSL
201 private float m_linearMotorTimescale = 0; // Motor Attack rate set by LSL
202 private float m_linearMotorDecayTimescale = 0; // Motor Decay rate set by LSL
203 private Vector3 m_linearFrictionTimescale = Vector3.Zero; // General Friction set by LSL
204
205 private Vector3 m_lLinMotorDVel = Vector3.Zero; // decayed motor
206 private Vector3 m_lLinObjectVel = Vector3.Zero; // local frame object velocity
207 private Vector3 m_wLinObjectVel = Vector3.Zero; // world frame object velocity
208
209 //Angular properties
210 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
211
212 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
213 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
214 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
215
216 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
217// private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity
218 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
219
220 private Vector3 m_angularLock = Vector3.One;
221
222 //Deflection properties
223 // private float m_angularDeflectionEfficiency = 0;
224 // private float m_angularDeflectionTimescale = 0;
225 // private float m_linearDeflectionEfficiency = 0;
226 // private float m_linearDeflectionTimescale = 0;
227
228 //Banking properties
229 // private float m_bankingEfficiency = 0;
230 // private float m_bankingMix = 0;
231 // private float m_bankingTimescale = 0;
232
233 //Hover and Buoyancy properties
234 private float m_VhoverHeight = 0f;
235// private float m_VhoverEfficiency = 0f;
236 private float m_VhoverTimescale = 0f;
237 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
238 private float m_VehicleBuoyancy = 0f; // Set by VEHICLE_BUOYANCY, for a vehicle.
239 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
240 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
241 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
242
243 //Attractor properties
244 private float m_verticalAttractionEfficiency = 1.0f; // damped
245 private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor.
246
247
248
249
250
207 251
208 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, 252 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
209 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode) 253 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode)
210 { 254 {
211 m_vehicle = new ODEDynamics();
212 //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
213 ode = dode; 255 ode = dode;
214 if (!pos.IsFinite()) 256 if (!pos.IsFinite())
215 { 257 {
@@ -302,7 +344,7 @@ namespace OpenSim.Region.Physics.OdePlugin
302 { 344 {
303 set { 345 set {
304 346
305 347//Console.WriteLine("Sel {0} {1} {2}", m_primName, value, m_isphysical);
306 // This only makes the object not collidable if the object 348 // This only makes the object not collidable if the object
307 // is physical or the object is modified somehow *IN THE FUTURE* 349 // is physical or the object is modified somehow *IN THE FUTURE*
308 // without this, if an avatar selects prim, they can walk right 350 // without this, if an avatar selects prim, they can walk right
@@ -322,6 +364,411 @@ namespace OpenSim.Region.Physics.OdePlugin
322 } 364 }
323 } 365 }
324 366
367 public override bool IsPhysical
368 {
369 get { return m_isphysical; }
370 set
371 {
372 m_isphysical = value;
373 if (!m_isphysical)
374 { // Zero the remembered last velocity
375 m_lastVelocity = Vector3.Zero;
376 if (m_type != Vehicle.TYPE_NONE) Halt();
377 }
378 }
379 }
380
381 public void setPrimForRemoval()
382 {
383 m_taintremove = true;
384 }
385
386 public override bool Flying
387 {
388 // no flying prims for you
389 get { return false; }
390 set { }
391 }
392
393 public override bool IsColliding
394 {
395 get { return iscolliding; }
396 set { iscolliding = value; }
397 }
398
399 public override bool CollidingGround
400 {
401 get { return false; }
402 set { return; }
403 }
404
405 public override bool CollidingObj
406 {
407 get { return false; }
408 set { return; }
409 }
410
411 public override bool ThrottleUpdates
412 {
413 get { return m_throttleUpdates; }
414 set { m_throttleUpdates = value; }
415 }
416
417 public override bool Stopped
418 {
419 get { return _zeroFlag; }
420 }
421
422 public override Vector3 Position
423 {
424 get { return _position; }
425
426 set { _position = value;
427 //m_log.Info("[PHYSICS]: " + _position.ToString());
428 }
429 }
430
431 public override Vector3 Size
432 {
433 get { return _size; }
434 set
435 {
436 if (value.IsFinite())
437 {
438 _size = value;
439 }
440 else
441 {
442 m_log.Warn("[PHYSICS]: Got NaN Size on object");
443 }
444 }
445 }
446
447 public override float Mass
448 {
449 get { return CalculateMass(); }
450 }
451
452 public override Vector3 Force
453 {
454 //get { return Vector3.Zero; }
455 get { return m_force; }
456 set
457 {
458 if (value.IsFinite())
459 {
460 m_force = value;
461 }
462 else
463 {
464 m_log.Warn("[PHYSICS]: NaN in Force Applied to an Object");
465 }
466 }
467 }
468
469 public override int VehicleType
470 {
471 get { return (int)m_type; }
472 set { ProcessTypeChange((Vehicle)value); }
473 }
474
475 public override void VehicleFloatParam(int param, float value)
476 {
477 ProcessFloatVehicleParam((Vehicle) param, value);
478 }
479
480 public override void VehicleVectorParam(int param, Vector3 value)
481 {
482 ProcessVectorVehicleParam((Vehicle) param, value);
483 }
484
485 public override void VehicleRotationParam(int param, Quaternion rotation)
486 {
487 ProcessRotationVehicleParam((Vehicle) param, rotation);
488 }
489
490 public override void VehicleFlags(int param, bool remove)
491 {
492 ProcessVehicleFlags(param, remove);
493 }
494
495 public override void SetVolumeDetect(int param)
496 {
497 lock (_parent_scene.OdeLock)
498 {
499 m_isVolumeDetect = (param!=0);
500 }
501 }
502
503 public override Vector3 CenterOfMass
504 {
505 get { return Vector3.Zero; }
506 }
507
508 public override Vector3 GeometricCenter
509 {
510 get { return Vector3.Zero; }
511 }
512
513 public override PrimitiveBaseShape Shape
514 {
515 set
516 {
517 _pbs = value;
518 m_taintshape = true;
519 }
520 }
521
522 public override Vector3 Velocity
523 {
524 get
525 {
526 // Averate previous velocity with the new one so
527 // client object interpolation works a 'little' better
528 if (_zeroFlag)
529 return Vector3.Zero;
530
531 Vector3 returnVelocity = Vector3.Zero;
532 returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2;
533 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2;
534 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2;
535 return returnVelocity;
536 }
537 set
538 {
539 if (value.IsFinite())
540 {
541 _velocity = value;
542
543 m_taintVelocity = value;
544 _parent_scene.AddPhysicsActorTaint(this);
545 }
546 else
547 {
548 m_log.Warn("[PHYSICS]: Got NaN Velocity in Object");
549 }
550
551 }
552 }
553
554 public override Vector3 Torque
555 {
556 get
557 {
558 if (!m_isphysical || Body == IntPtr.Zero)
559 return Vector3.Zero;
560
561 return _torque;
562 }
563
564 set
565 {
566 if (value.IsFinite())
567 {
568 m_taintTorque = value;
569 _parent_scene.AddPhysicsActorTaint(this);
570 }
571 else
572 {
573 m_log.Warn("[PHYSICS]: Got NaN Torque in Object");
574 }
575 }
576 }
577
578 public override float CollisionScore
579 {
580 get { return m_collisionscore; }
581 set { m_collisionscore = value; }
582 }
583
584 public override bool Kinematic
585 {
586 get { return false; }
587 set { }
588 }
589
590 public override Quaternion Orientation
591 {
592 get { return _orientation; }
593 set
594 {
595 if (QuaternionIsFinite(value))
596 {
597 _orientation = value;
598 }
599 else
600 m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object");
601
602 }
603 }
604
605
606 public override bool FloatOnWater
607 {
608 set {
609 m_taintCollidesWater = value;
610 _parent_scene.AddPhysicsActorTaint(this);
611 }
612 }
613
614 public override void SetMomentum(Vector3 momentum)
615 {
616 }
617
618 public override Vector3 PIDTarget
619 {
620 set
621 {
622 if (value.IsFinite())
623 {
624 m_PIDTarget = value;
625 }
626 else
627 m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object");
628 }
629 }
630 public override bool PIDActive { set { m_usePID = value; } }
631 public override float PIDTau { set { m_PIDTau = value; } }
632
633 // For RotLookAt
634 public override Quaternion APIDTarget { set { m_APIDTarget = value; } }
635 public override bool APIDActive { set { m_useAPID = value; } }
636 public override float APIDStrength { set { m_APIDStrength = value; } }
637 public override float APIDDamping { set { m_APIDDamping = value; } }
638
639 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
640 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
641 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
642 public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
643
644 internal static bool QuaternionIsFinite(Quaternion q)
645 {
646 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
647 return false;
648 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
649 return false;
650 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
651 return false;
652 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
653 return false;
654 return true;
655 }
656
657 public override Vector3 Acceleration
658 {
659 get { return _acceleration; }
660 }
661
662
663 public void SetAcceleration(Vector3 accel) // No one calls this, and it would not do anything.
664 {
665 _acceleration = accel;
666 }
667
668 public override void AddForce(Vector3 force, bool pushforce)
669 {
670 if (force.IsFinite())
671 {
672 lock (m_forcelist)
673 m_forcelist.Add(force);
674
675 m_taintforce = true;
676 }
677 else
678 {
679 m_log.Warn("[PHYSICS]: Got Invalid linear force vector from Scene in Object");
680 }
681 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
682 }
683
684 public override void AddAngularForce(Vector3 force, bool pushforce)
685 {
686 if (force.IsFinite())
687 {
688 m_angularforcelist.Add(force);
689 m_taintaddangularforce = true;
690 }
691 else
692 {
693 m_log.Warn("[PHYSICS]: Got Invalid Angular force vector from Scene in Object");
694 }
695 }
696
697 public override Vector3 RotationalVelocity
698 {
699 get
700 {
701/* Vector3 pv = Vector3.Zero;
702 if (_zeroFlag)
703 return pv;
704 m_lastUpdateSent = false;
705
706 if (m_rotationalVelocity.ApproxEquals(pv, 0.2f))
707 return pv;
708*/
709 return m_rotationalVelocity;
710 }
711 set
712 {
713 if (value.IsFinite())
714 {
715 m_rotationalVelocity = value;
716 }
717 else
718 {
719 m_log.Warn("[PHYSICS]: Got NaN RotationalVelocity in Object");
720 }
721 }
722 }
723
724 public override void CrossingFailure()
725 {
726 m_crossingfailures++;
727 if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
728 {
729 base.RaiseOutOfBounds(_position);
730 return;
731 }
732 else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
733 {
734 m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);
735 }
736 }
737
738 public override float Buoyancy
739 {
740 get { return m_buoyancy; }
741 set { m_buoyancy = value; }
742 }
743
744 public override void link(PhysicsActor obj)
745 {
746 m_taintparent = obj;
747 }
748
749 public override void delink()
750 {
751 m_taintparent = null;
752 }
753
754 public override void LockAngularMotion(Vector3 axis)
755 {
756 // reverse the zero/non zero values for ODE.
757 if (axis.IsFinite())
758 {
759 axis.X = (axis.X > 0) ? 1f : 0f;
760 axis.Y = (axis.Y > 0) ? 1f : 0f;
761 axis.Z = (axis.Z > 0) ? 1f : 0f;
762 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
763 m_taintAngularLock = axis;
764 }
765 else
766 {
767 m_log.Warn("[PHYSICS]: Got NaN locking axis from Scene on Object");
768 }
769 }
770
771
325 public void SetGeom(IntPtr geom) 772 public void SetGeom(IntPtr geom)
326 { 773 {
327 prev_geom = prim_geom; 774 prev_geom = prim_geom;
@@ -345,8 +792,6 @@ namespace OpenSim.Region.Physics.OdePlugin
345 //m_log.Warn("Setting Geom to: " + prim_geom); 792 //m_log.Warn("Setting Geom to: " + prim_geom);
346 } 793 }
347 794
348
349
350 public void enableBodySoft() 795 public void enableBodySoft()
351 { 796 {
352 if (!childPrim) 797 if (!childPrim)
@@ -354,8 +799,8 @@ namespace OpenSim.Region.Physics.OdePlugin
354 if (m_isphysical && Body != IntPtr.Zero) 799 if (m_isphysical && Body != IntPtr.Zero)
355 { 800 {
356 d.BodyEnable(Body); 801 d.BodyEnable(Body);
357 if (m_vehicle.Type != Vehicle.TYPE_NONE) 802 if (m_type != Vehicle.TYPE_NONE)
358 m_vehicle.Enable(Body, _parent_scene); 803 Enable(Body, _parent_scene);
359 } 804 }
360 805
361 m_disabled = false; 806 m_disabled = false;
@@ -406,14 +851,9 @@ namespace OpenSim.Region.Physics.OdePlugin
406 m_collisionscore = 0; 851 m_collisionscore = 0;
407 m_disabled = false; 852 m_disabled = false;
408 853
409 // The body doesn't already have a finite rotation mode set here 854 if (m_type != Vehicle.TYPE_NONE)
410 /* ### if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0.0f)) && _parent == null)
411 { 855 {
412 createAMotor(m_angularlock); 856 Enable(Body, _parent_scene);
413 } */
414 if (m_vehicle.Type != Vehicle.TYPE_NONE)
415 {
416 m_vehicle.Enable(Body, _parent_scene);
417 } 857 }
418 858
419 _parent_scene.addActivePrim(this); 859 _parent_scene.addActivePrim(this);
@@ -894,9 +1334,8 @@ namespace OpenSim.Region.Physics.OdePlugin
894 // } 1334 // }
895 } 1335 }
896 1336
897 public void ProcessTaints(float timestep) 1337 public void ProcessTaints(float timestep) //=============================================================================
898 { 1338 {
899//Console.WriteLine("ProcessTaints for " + m_primName );
900 if (m_taintadd) 1339 if (m_taintadd)
901 { 1340 {
902 changeadd(timestep); 1341 changeadd(timestep);
@@ -985,30 +1424,6 @@ namespace OpenSim.Region.Physics.OdePlugin
985 { 1424 {
986//Console.WriteLine("Alock changed to {0}", m_taintAngularLock); 1425//Console.WriteLine("Alock changed to {0}", m_taintAngularLock);
987 m_angularlock = m_taintAngularLock; 1426 m_angularlock = m_taintAngularLock;
988 m_vehicle.SetAngularLock(m_angularlock);
989
990
991/*
992 if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f))
993 {
994 //d.BodySetFiniteRotationMode(Body, 0);
995 //d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z);
996 createAMotor(m_taintAngularLock);
997 }
998 else
999 {
1000 if (Amotor != IntPtr.Zero)
1001 {
1002 d.JointDestroy(Amotor);
1003 Amotor = IntPtr.Zero;
1004 }
1005 }
1006 }
1007 }
1008 // Store this for later in case we get turned into a separate body
1009 m_angularlock = m_taintAngularLock;
1010 m_vehicle.SetAngularLock(m_angularlock);
1011 } */
1012 } 1427 }
1013 } 1428 }
1014 1429
@@ -1151,11 +1566,6 @@ namespace OpenSim.Region.Physics.OdePlugin
1151 prm.m_collisionscore = 0; 1566 prm.m_collisionscore = 0;
1152 prm.m_disabled = false; 1567 prm.m_disabled = false;
1153 1568
1154 // The body doesn't already have a finite rotation mode set here
1155 /* ### if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
1156 {
1157 prm.createAMotor(m_angularlock);
1158 } */
1159 prm.Body = Body; 1569 prm.Body = Body;
1160 _parent_scene.addActivePrim(prm); 1570 _parent_scene.addActivePrim(prm);
1161 } 1571 }
@@ -1194,13 +1604,8 @@ namespace OpenSim.Region.Physics.OdePlugin
1194 m_collisionscore = 0; 1604 m_collisionscore = 0;
1195 m_disabled = false; 1605 m_disabled = false;
1196 1606
1197 // The body doesn't already have a finite rotation mode set here
1198 /* ### if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
1199 {
1200 createAMotor(m_angularlock);
1201 } */
1202 d.BodySetPosition(Body, Position.X, Position.Y, Position.Z); 1607 d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
1203 if (m_vehicle.Type != Vehicle.TYPE_NONE) m_vehicle.Enable(Body, _parent_scene); 1608 if (m_type != Vehicle.TYPE_NONE) Enable(Body, _parent_scene);
1204 _parent_scene.addActivePrim(this); 1609 _parent_scene.addActivePrim(this);
1205 } 1610 }
1206 } 1611 }
@@ -1267,17 +1672,12 @@ namespace OpenSim.Region.Physics.OdePlugin
1267 //Console.WriteLine("childrenPrim.Remove " + odePrim); 1672 //Console.WriteLine("childrenPrim.Remove " + odePrim);
1268 childrenPrim.Remove(odePrim); 1673 childrenPrim.Remove(odePrim);
1269 } 1674 }
1270
1271
1272
1273 1675
1274 if (Body != IntPtr.Zero) 1676 if (Body != IntPtr.Zero)
1275 { 1677 {
1276 _parent_scene.remActivePrim(this); 1678 _parent_scene.remActivePrim(this);
1277 } 1679 }
1278 1680
1279
1280
1281 lock (childrenPrim) 1681 lock (childrenPrim)
1282 { 1682 {
1283 foreach (OdePrim prm in childrenPrim) 1683 foreach (OdePrim prm in childrenPrim)
@@ -1286,8 +1686,6 @@ namespace OpenSim.Region.Physics.OdePlugin
1286 ParentPrim(prm); 1686 ParentPrim(prm);
1287 } 1687 }
1288 } 1688 }
1289
1290
1291 } 1689 }
1292 1690
1293 private void changeSelectedStatus(float timestep) 1691 private void changeSelectedStatus(float timestep)
@@ -1515,17 +1913,22 @@ namespace OpenSim.Region.Physics.OdePlugin
1515 1913
1516 public void changemove(float timestep) 1914 public void changemove(float timestep)
1517 { 1915 {
1916//Console.WriteLine("changemove for {0}", m_primName );
1917
1518 if (m_isphysical) 1918 if (m_isphysical)
1519 { 1919 {
1520 1920//Console.WriteLine("phys {0} {1} {2}", m_disabled, m_taintremove, childPrim);
1521 if (!m_disabled && !m_taintremove && !childPrim) 1921// if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits!
1922 if (!m_taintremove && !childPrim)
1522 { 1923 {
1924//Console.WriteLine("physOK");
1523 if (Body == IntPtr.Zero) 1925 if (Body == IntPtr.Zero)
1524 enableBody(); 1926 enableBody();
1525 //Prim auto disable after 20 frames, 1927 //Prim auto disable after 20 frames,
1526 //if you move it, re-enable the prim manually. 1928 //if you move it, re-enable the prim manually.
1527 if (_parent != null) 1929 if (_parent != null)
1528 { 1930 {
1931//Console.WriteLine("physChild");
1529 if (m_linkJoint != IntPtr.Zero) 1932 if (m_linkJoint != IntPtr.Zero)
1530 { 1933 {
1531 d.JointDestroy(m_linkJoint); 1934 d.JointDestroy(m_linkJoint);
@@ -1534,6 +1937,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1534 } 1937 }
1535 if (Body != IntPtr.Zero) 1938 if (Body != IntPtr.Zero)
1536 { 1939 {
1940//Console.WriteLine("physNotIPZ");
1537 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); 1941 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
1538 1942
1539 if (_parent != null) 1943 if (_parent != null)
@@ -1549,9 +1953,9 @@ Console.WriteLine(" JointCreateFixed");
1549 } 1953 }
1550 } 1954 }
1551 d.BodyEnable(Body); 1955 d.BodyEnable(Body);
1552 if (m_vehicle.Type != Vehicle.TYPE_NONE) 1956 if (m_type != Vehicle.TYPE_NONE)
1553 { 1957 {
1554 m_vehicle.Enable(Body, _parent_scene); 1958 Enable(Body, _parent_scene);
1555 } 1959 }
1556 } 1960 }
1557 else 1961 else
@@ -1566,6 +1970,7 @@ Console.WriteLine(" JointCreateFixed");
1566 } 1970 }
1567 else 1971 else
1568 { 1972 {
1973//Console.WriteLine("NONphys");
1569 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); 1974 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
1570 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); 1975 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
1571 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1976 _parent_scene.waitForSpaceUnlock(m_targetSpace);
@@ -1589,308 +1994,6 @@ Console.WriteLine(" JointCreateFixed");
1589 m_taintposition = _position; 1994 m_taintposition = _position;
1590 } 1995 }
1591 1996
1592 public void Move(float timestep)
1593 {
1594 float fx = 0;
1595 float fy = 0;
1596 float fz = 0;
1597
1598 frcount++; // used to limit debug comment output
1599 if (frcount > 100)
1600 frcount = 0;
1601
1602 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
1603 {
1604//if(frcount == 0) Console.WriteLine("Move " + m_primName + " VTyp " + m_vehicle.Type +
1605 // " usePID=" + m_usePID + " seHover=" + m_useHoverPID + " useAPID=" + m_useAPID);
1606 if (m_vehicle.Type != Vehicle.TYPE_NONE)
1607 {
1608 // 'VEHICLES' are dealt with in ODEDynamics.cs
1609 m_vehicle.Step(timestep, _parent_scene);
1610 }
1611 else
1612 {
1613 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
1614 // NON-'VEHICLES' are dealt with here
1615 // m_angularlock = <1,1,1> means no lock. a 0 on axis means locked.
1616
1617// NB this may be wrong - may lock global axis! Should be LOCAL axis!
1618 if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.One, 0.003f))
1619 {
1620 d.Vector3 avel2 = d.BodyGetAngularVel(Body);
1621 if (m_angularlock.X == 0)
1622 avel2.X = 0;
1623 if (m_angularlock.Y == 0)
1624 avel2.Y = 0;
1625 if (m_angularlock.Z == 0)
1626 avel2.Z = 0;
1627 d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
1628 }
1629 //float PID_P = 900.0f;
1630
1631 float m_mass = CalculateMass();
1632
1633// fz = 0f;
1634 //m_log.Info(m_collisionFlags.ToString());
1635
1636
1637 //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle.
1638 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
1639 // NB Prims in ODE are no subject to global gravity
1640 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass
1641
1642 if (m_usePID)
1643 {
1644//if(frcount == 0) Console.WriteLine("PID " + m_primName);
1645 // KF - this is for object MoveToTarget.
1646
1647 //if (!d.BodyIsEnabled(Body))
1648 //d.BodySetForce(Body, 0f, 0f, 0f);
1649
1650 // no lock; for now it's only called from within Simulate()
1651
1652 // If the PID Controller isn't active then we set our force
1653 // calculating base velocity to the current position
1654
1655 if ((m_PIDTau < 1) && (m_PIDTau != 0))
1656 {
1657 //PID_G = PID_G / m_PIDTau;
1658 m_PIDTau = 1;
1659 }
1660
1661 if ((PID_G - m_PIDTau) <= 0)
1662 {
1663 PID_G = m_PIDTau + 1;
1664 }
1665 //PidStatus = true;
1666
1667 // PhysicsVector vec = new PhysicsVector();
1668 d.Vector3 vel = d.BodyGetLinearVel(Body);
1669
1670 d.Vector3 pos = d.BodyGetPosition(Body);
1671 _target_velocity =
1672 new Vector3(
1673 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
1674 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
1675 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
1676 );
1677
1678 // if velocity is zero, use position control; otherwise, velocity control
1679
1680 if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
1681 {
1682 // keep track of where we stopped. No more slippin' & slidin'
1683
1684 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1685 // react to the physics scene by moving it's position.
1686 // Avatar to Avatar collisions
1687 // Prim to avatar collisions
1688
1689 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
1690 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
1691 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
1692 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
1693 d.BodySetLinearVel(Body, 0, 0, 0);
1694 d.BodyAddForce(Body, 0, 0, fz);
1695 return;
1696 }
1697 else
1698 {
1699 _zeroFlag = false;
1700
1701 // We're flying and colliding with something
1702 fx = ((_target_velocity.X) - vel.X) * (PID_D);
1703 fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
1704
1705 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
1706
1707 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1708 }
1709 } // end if (m_usePID)
1710
1711 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
1712 if (m_useHoverPID && !m_usePID)
1713 {
1714//Console.WriteLine("Hover " + m_primName);
1715
1716 // If we're using the PID controller, then we have no gravity
1717 fz = (-1 * _parent_scene.gravityz) * m_mass;
1718
1719 // no lock; for now it's only called from within Simulate()
1720
1721 // If the PID Controller isn't active then we set our force
1722 // calculating base velocity to the current position
1723
1724 if ((m_PIDTau < 1))
1725 {
1726 PID_G = PID_G / m_PIDTau;
1727 }
1728
1729 if ((PID_G - m_PIDTau) <= 0)
1730 {
1731 PID_G = m_PIDTau + 1;
1732 }
1733
1734
1735 // Where are we, and where are we headed?
1736 d.Vector3 pos = d.BodyGetPosition(Body);
1737 d.Vector3 vel = d.BodyGetLinearVel(Body);
1738
1739
1740 // Non-Vehicles have a limited set of Hover options.
1741 // determine what our target height really is based on HoverType
1742 switch (m_PIDHoverType)
1743 {
1744 case PIDHoverType.Ground:
1745 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1746 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1747 break;
1748 case PIDHoverType.GroundAndWater:
1749 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1750 m_waterHeight = _parent_scene.GetWaterLevel();
1751 if (m_groundHeight > m_waterHeight)
1752 {
1753 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1754 }
1755 else
1756 {
1757 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
1758 }
1759 break;
1760
1761 } // end switch (m_PIDHoverType)
1762
1763
1764 _target_velocity =
1765 new Vector3(0.0f, 0.0f,
1766 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
1767 );
1768
1769 // if velocity is zero, use position control; otherwise, velocity control
1770
1771 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
1772 {
1773 // keep track of where we stopped. No more slippin' & slidin'
1774
1775 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1776 // react to the physics scene by moving it's position.
1777 // Avatar to Avatar collisions
1778 // Prim to avatar collisions
1779
1780 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
1781 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
1782 d.BodyAddForce(Body, 0, 0, fz);
1783 //KF this prevents furthur motions return;
1784 }
1785 else
1786 {
1787 _zeroFlag = false;
1788
1789 // We're flying and colliding with something
1790 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1791 }
1792 } // end m_useHoverPID && !m_usePID
1793
1794 if (m_useAPID)
1795 {
1796 // RotLookAt, apparently overrides all other rotation sources. Inputs:
1797 // Quaternion m_APIDTarget
1798 // float m_APIDStrength // From SL experiments, this is the time to get there
1799 // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
1800 // Also in SL the mass of the object has no effect on time to get there.
1801 // Factors:
1802//if(frcount == 0) Console.WriteLine("APID ");
1803 // get present body rotation
1804 float limit = 1.0f;
1805 float scaler = 50f; // adjusts damping time
1806 float RLAservo = 0f;
1807
1808 d.Quaternion rot = d.BodyGetQuaternion(Body);
1809 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
1810 Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget;
1811 float diff_angle;
1812 Vector3 diff_axis;
1813 rot_diff.GetAxisAngle(out diff_axis, out diff_angle);
1814 diff_axis.Normalize();
1815 if(diff_angle > 0.01f) // diff_angle is always +ve
1816 {
1817// PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z);
1818 Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
1819 rotforce = rotforce * rotq;
1820 if(diff_angle > limit) diff_angle = limit; // cap the rotate rate
1821// RLAservo = timestep / m_APIDStrength * m_mass * scaler;
1822 // rotforce = rotforce * RLAservo * diff_angle ;
1823 // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z);
1824 RLAservo = timestep / m_APIDStrength * scaler;
1825 rotforce = rotforce * RLAservo * diff_angle ;
1826
1827 if (m_angularlock.X == 0)
1828 rotforce.X = 0;
1829 if (m_angularlock.Y == 0)
1830 rotforce.Y = 0;
1831 if (m_angularlock.Z == 0)
1832 rotforce.Z = 0;
1833
1834
1835
1836
1837 d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z);
1838//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo);
1839 }
1840//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle);
1841 } // end m_useAPID
1842
1843 fx *= m_mass;
1844 fy *= m_mass;
1845 //fz *= m_mass;
1846
1847 fx += m_force.X;
1848 fy += m_force.Y;
1849 fz += m_force.Z;
1850
1851 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
1852 if (fx != 0 || fy != 0 || fz != 0)
1853 {
1854 //m_taintdisable = true;
1855 //base.RaiseOutOfBounds(Position);
1856 //d.BodySetLinearVel(Body, fx, fy, 0f);
1857 if (!d.BodyIsEnabled(Body))
1858 {
1859 // A physical body at rest on a surface will auto-disable after a while,
1860 // this appears to re-enable it incase the surface it is upon vanishes,
1861 // and the body should fall again.
1862 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1863 d.BodySetForce(Body, 0, 0, 0);
1864 enableBodySoft();
1865 }
1866
1867 // 35x10 = 350n times the mass per second applied maximum.
1868 float nmax = 35f * m_mass;
1869 float nmin = -35f * m_mass;
1870
1871
1872 if (fx > nmax)
1873 fx = nmax;
1874 if (fx < nmin)
1875 fx = nmin;
1876 if (fy > nmax)
1877 fy = nmax;
1878 if (fy < nmin)
1879 fy = nmin;
1880 d.BodyAddForce(Body, fx, fy, fz);
1881//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
1882 }
1883 }
1884 }
1885 else
1886 { // is not physical, or is not a body or is selected
1887 // _zeroPosition = d.BodyGetPosition(Body);
1888 return;
1889//Console.WriteLine("Nothing " + m_primName);
1890
1891 }
1892 }
1893
1894 1997
1895 1998
1896 public void rotate(float timestep) 1999 public void rotate(float timestep)
@@ -1904,11 +2007,6 @@ Console.WriteLine(" JointCreateFixed");
1904 { 2007 {
1905 // KF: If this is a root prim do BodySet 2008 // KF: If this is a root prim do BodySet
1906 d.BodySetQuaternion(Body, ref myrot); 2009 d.BodySetQuaternion(Body, ref myrot);
1907 /* ### if (m_isphysical)
1908 {
1909 if (!m_angularlock.ApproxEquals(Vector3.One, 0f))
1910 createAMotor(m_angularlock);
1911 } */
1912 } 2010 }
1913 else 2011 else
1914 { 2012 {
@@ -2319,395 +2417,12 @@ Console.WriteLine(" JointCreateFixed");
2319 m_taintVelocity = Vector3.Zero; 2417 m_taintVelocity = Vector3.Zero;
2320 } 2418 }
2321 2419
2322 public override bool IsPhysical
2323 {
2324 get { return m_isphysical; }
2325 set
2326 {
2327 m_isphysical = value;
2328 if (!m_isphysical)
2329 { // Zero the remembered last velocity
2330 m_lastVelocity = Vector3.Zero;
2331 if (m_vehicle.Type != Vehicle.TYPE_NONE) m_vehicle.Halt();
2332 }
2333 }
2334 }
2335
2336 public void setPrimForRemoval()
2337 {
2338 m_taintremove = true;
2339 }
2340
2341 public override bool Flying
2342 {
2343 // no flying prims for you
2344 get { return false; }
2345 set { }
2346 }
2347
2348 public override bool IsColliding
2349 {
2350 get { return iscolliding; }
2351 set { iscolliding = value; }
2352 }
2353
2354 public override bool CollidingGround
2355 {
2356 get { return false; }
2357 set { return; }
2358 }
2359
2360 public override bool CollidingObj
2361 {
2362 get { return false; }
2363 set { return; }
2364 }
2365
2366 public override bool ThrottleUpdates
2367 {
2368 get { return m_throttleUpdates; }
2369 set { m_throttleUpdates = value; }
2370 }
2371
2372 public override bool Stopped
2373 {
2374 get { return _zeroFlag; }
2375 }
2376
2377 public override Vector3 Position
2378 {
2379 get { return _position; }
2380
2381 set { _position = value;
2382 //m_log.Info("[PHYSICS]: " + _position.ToString());
2383 }
2384 }
2385
2386 public override Vector3 Size
2387 {
2388 get { return _size; }
2389 set
2390 {
2391 if (value.IsFinite())
2392 {
2393 _size = value;
2394 }
2395 else
2396 {
2397 m_log.Warn("[PHYSICS]: Got NaN Size on object");
2398 }
2399 }
2400 }
2401
2402 public override float Mass
2403 {
2404 get { return CalculateMass(); }
2405 }
2406
2407 public override Vector3 Force
2408 {
2409 //get { return Vector3.Zero; }
2410 get { return m_force; }
2411 set
2412 {
2413 if (value.IsFinite())
2414 {
2415 m_force = value;
2416 }
2417 else
2418 {
2419 m_log.Warn("[PHYSICS]: NaN in Force Applied to an Object");
2420 }
2421 }
2422 }
2423
2424 public override int VehicleType
2425 {
2426 get { return (int)m_vehicle.Type; }
2427 set { m_vehicle.ProcessTypeChange((Vehicle)value); }
2428 }
2429
2430 public override void VehicleFloatParam(int param, float value)
2431 {
2432 m_vehicle.ProcessFloatVehicleParam((Vehicle) param, value);
2433 }
2434
2435 public override void VehicleVectorParam(int param, Vector3 value)
2436 {
2437 m_vehicle.ProcessVectorVehicleParam((Vehicle) param, value);
2438 }
2439
2440 public override void VehicleRotationParam(int param, Quaternion rotation)
2441 {
2442 m_vehicle.ProcessRotationVehicleParam((Vehicle) param, rotation);
2443 }
2444
2445 public override void VehicleFlags(int flags, bool remove)
2446 {
2447 if (!remove)
2448 VehicleFlagsSet(flags);
2449 else
2450 VehicleFlagsRemove(flags);
2451 }
2452
2453 public override void VehicleFlagsSet(int flags)
2454 {
2455 m_vehicle.ProcessFlagsVehicleSet(flags);
2456 }
2457
2458 public override void VehicleFlagsRemove(int flags)
2459 {
2460 m_vehicle.ProcessFlagsVehicleRemove(flags);
2461 }
2462
2463 public override void SetVolumeDetect(int param)
2464 {
2465 lock (_parent_scene.OdeLock)
2466 {
2467 m_isVolumeDetect = (param!=0);
2468 }
2469 }
2470
2471 public override Vector3 CenterOfMass
2472 {
2473 get { return Vector3.Zero; }
2474 }
2475
2476 public override Vector3 GeometricCenter
2477 {
2478 get { return Vector3.Zero; }
2479 }
2480
2481 public override PrimitiveBaseShape Shape
2482 {
2483 set
2484 {
2485 _pbs = value;
2486 m_taintshape = true;
2487 }
2488 }
2489
2490 public override Vector3 Velocity
2491 {
2492 get
2493 {
2494 // Averate previous velocity with the new one so
2495 // client object interpolation works a 'little' better
2496 if (_zeroFlag)
2497 return Vector3.Zero;
2498
2499 Vector3 returnVelocity = Vector3.Zero;
2500 returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2;
2501 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2;
2502 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2;
2503 return returnVelocity;
2504 }
2505 set
2506 {
2507 if (value.IsFinite())
2508 {
2509 _velocity = value;
2510
2511 m_taintVelocity = value;
2512 _parent_scene.AddPhysicsActorTaint(this);
2513 }
2514 else
2515 {
2516 m_log.Warn("[PHYSICS]: Got NaN Velocity in Object");
2517 }
2518
2519 }
2520 }
2521
2522 public override Vector3 Torque
2523 {
2524 get
2525 {
2526 if (!m_isphysical || Body == IntPtr.Zero)
2527 return Vector3.Zero;
2528
2529 return _torque;
2530 }
2531
2532 set
2533 {
2534 if (value.IsFinite())
2535 {
2536 m_taintTorque = value;
2537 _parent_scene.AddPhysicsActorTaint(this);
2538 }
2539 else
2540 {
2541 m_log.Warn("[PHYSICS]: Got NaN Torque in Object");
2542 }
2543 }
2544 }
2545/*
2546 public Vector3 AngularLock
2547 {
2548 get { return m_angularlock; }
2549 set { }
2550 }
2551
2552*/
2553 public override float CollisionScore
2554 {
2555 get { return m_collisionscore; }
2556 set { m_collisionscore = value; }
2557 }
2558
2559 public override bool Kinematic
2560 {
2561 get { return false; }
2562 set { }
2563 }
2564
2565 public override Quaternion Orientation
2566 {
2567 get { return _orientation; }
2568 set
2569 {
2570 if (QuaternionIsFinite(value))
2571 {
2572 _orientation = value;
2573 }
2574 else
2575 m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object");
2576
2577 }
2578 }
2579
2580 internal static bool QuaternionIsFinite(Quaternion q)
2581 {
2582 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
2583 return false;
2584 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
2585 return false;
2586 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
2587 return false;
2588 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
2589 return false;
2590 return true;
2591 }
2592
2593 public override Vector3 Acceleration
2594 {
2595 get { return _acceleration; }
2596 }
2597
2598
2599 public void SetAcceleration(Vector3 accel)
2600 {
2601 _acceleration = accel;
2602 }
2603
2604 public override void AddForce(Vector3 force, bool pushforce)
2605 {
2606 if (force.IsFinite())
2607 {
2608 lock (m_forcelist)
2609 m_forcelist.Add(force);
2610
2611 m_taintforce = true;
2612 }
2613 else
2614 {
2615 m_log.Warn("[PHYSICS]: Got Invalid linear force vector from Scene in Object");
2616 }
2617 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
2618 }
2619
2620 public override void AddAngularForce(Vector3 force, bool pushforce)
2621 {
2622 if (force.IsFinite())
2623 {
2624 m_angularforcelist.Add(force);
2625 m_taintaddangularforce = true;
2626 }
2627 else
2628 {
2629 m_log.Warn("[PHYSICS]: Got Invalid Angular force vector from Scene in Object");
2630 }
2631 }
2632
2633 public override Vector3 RotationalVelocity
2634 {
2635 get
2636 {
2637/* Vector3 pv = Vector3.Zero;
2638 if (_zeroFlag)
2639 return pv;
2640 m_lastUpdateSent = false;
2641
2642 if (m_rotationalVelocity.ApproxEquals(pv, 0.2f))
2643 return pv;
2644*/
2645 return m_rotationalVelocity;
2646 }
2647 set
2648 {
2649 if (value.IsFinite())
2650 {
2651 m_rotationalVelocity = value;
2652 }
2653 else
2654 {
2655 m_log.Warn("[PHYSICS]: Got NaN RotationalVelocity in Object");
2656 }
2657 }
2658 }
2659
2660 public override void CrossingFailure()
2661 {
2662 m_crossingfailures++;
2663 if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
2664 {
2665 base.RaiseOutOfBounds(_position);
2666 return;
2667 }
2668 else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
2669 {
2670 m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);
2671 }
2672 }
2673
2674 public override float Buoyancy
2675 {
2676 get { return m_buoyancy; }
2677 set { m_buoyancy = value; }
2678 }
2679
2680 public override void link(PhysicsActor obj)
2681 {
2682 m_taintparent = obj;
2683 }
2684
2685 public override void delink()
2686 {
2687 m_taintparent = null;
2688 }
2689
2690 public override void LockAngularMotion(Vector3 axis)
2691 {
2692 // reverse the zero/non zero values for ODE.
2693 if (axis.IsFinite())
2694 {
2695 axis.X = (axis.X > 0) ? 1f : 0f;
2696 axis.Y = (axis.Y > 0) ? 1f : 0f;
2697 axis.Z = (axis.Z > 0) ? 1f : 0f;
2698 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
2699 m_taintAngularLock = axis;
2700 }
2701 else
2702 {
2703 m_log.Warn("[PHYSICS]: Got NaN locking axis from Scene on Object");
2704 }
2705 }
2706
2707 public void UpdatePositionAndVelocity() 2420 public void UpdatePositionAndVelocity()
2708 { 2421 {
2422 return; // moved to the MOVE() method
2423 }
2709 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! 2424 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
2710 if (_parent == null) 2425/* if (_parent == null)
2711 { 2426 {
2712 Vector3 pv = Vector3.Zero; 2427 Vector3 pv = Vector3.Zero;
2713 bool lastZeroFlag = _zeroFlag; 2428 bool lastZeroFlag = _zeroFlag;
@@ -2916,168 +2631,7 @@ Console.WriteLine(" JointCreateFixed");
2916 } 2631 }
2917 } 2632 }
2918 } 2633 }
2919 2634*/
2920 public override bool FloatOnWater
2921 {
2922 set {
2923 m_taintCollidesWater = value;
2924 _parent_scene.AddPhysicsActorTaint(this);
2925 }
2926 }
2927
2928 public override void SetMomentum(Vector3 momentum)
2929 {
2930 }
2931
2932 public override Vector3 PIDTarget
2933 {
2934 set
2935 {
2936 if (value.IsFinite())
2937 {
2938 m_PIDTarget = value;
2939 }
2940 else
2941 m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object");
2942 }
2943 }
2944 public override bool PIDActive { set { m_usePID = value; } }
2945 public override float PIDTau { set { m_PIDTau = value; } }
2946
2947 // For RotLookAt
2948 public override Quaternion APIDTarget { set { m_APIDTarget = value; } }
2949 public override bool APIDActive { set { m_useAPID = value; } }
2950 public override float APIDStrength { set { m_APIDStrength = value; } }
2951 public override float APIDDamping { set { m_APIDDamping = value; } }
2952
2953 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
2954 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
2955 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
2956 public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
2957
2958 private void createAMotor(Vector3 axis) // ##*
2959 {
2960Console.WriteLine(" createAMotor called! ----------------------------");
2961 if (Body == IntPtr.Zero)
2962 return;
2963
2964 if (Amotor != IntPtr.Zero)
2965 {
2966 d.JointDestroy(Amotor);
2967 Amotor = IntPtr.Zero;
2968 }
2969
2970 float axisnum = 3;
2971
2972 axisnum = (axisnum - (axis.X + axis.Y + axis.Z));
2973
2974 // PhysicsVector totalSize = new PhysicsVector(_size.X, _size.Y, _size.Z);
2975
2976
2977 // Inverse Inertia Matrix, set the X, Y, and/r Z inertia to 0 then invert it again.
2978 d.Mass objMass;
2979 d.MassSetZero(out objMass);
2980 DMassCopy(ref pMass, ref objMass);
2981
2982 //m_log.DebugFormat("1-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
2983
2984 Matrix4 dMassMat = FromDMass(objMass);
2985
2986 Matrix4 mathmat = Inverse(dMassMat);
2987
2988 /*
2989 //m_log.DebugFormat("2-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", mathmat[0, 0], mathmat[0, 1], mathmat[0, 2], mathmat[1, 0], mathmat[1, 1], mathmat[1, 2], mathmat[2, 0], mathmat[2, 1], mathmat[2, 2]);
2990
2991 mathmat = Inverse(mathmat);
2992
2993
2994 objMass = FromMatrix4(mathmat, ref objMass);
2995 //m_log.DebugFormat("3-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
2996
2997 mathmat = Inverse(mathmat);
2998 */
2999 if (axis.X == 0)
3000 {
3001 mathmat.M33 = 50.0000001f;
3002 //objMass.I.M22 = 0;
3003 }
3004 if (axis.Y == 0)
3005 {
3006 mathmat.M22 = 50.0000001f;
3007 //objMass.I.M11 = 0;
3008 }
3009 if (axis.Z == 0)
3010 {
3011 mathmat.M11 = 50.0000001f;
3012 //objMass.I.M00 = 0;
3013 }
3014
3015
3016
3017 mathmat = Inverse(mathmat);
3018 objMass = FromMatrix4(mathmat, ref objMass);
3019 //m_log.DebugFormat("4-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
3020
3021 //return;
3022 if (d.MassCheck(ref objMass))
3023 {
3024 d.BodySetMass(Body, ref objMass);
3025 }
3026 else
3027 {
3028 //m_log.Debug("[PHYSICS]: Mass invalid, ignoring");
3029 }
3030
3031 if (axisnum <= 0)
3032 return;
3033 // int dAMotorEuler = 1;
3034
3035 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
3036 d.JointAttach(Amotor, Body, IntPtr.Zero);
3037 d.JointSetAMotorMode(Amotor, 0);
3038
3039 d.JointSetAMotorNumAxes(Amotor,(int)axisnum);
3040 int i = 0;
3041
3042 if (axis.X == 0)
3043 {
3044 d.JointSetAMotorAxis(Amotor, i, 0, 1, 0, 0);
3045 i++;
3046 }
3047
3048 if (axis.Y == 0)
3049 {
3050 d.JointSetAMotorAxis(Amotor, i, 0, 0, 1, 0);
3051 i++;
3052 }
3053
3054 if (axis.Z == 0)
3055 {
3056 d.JointSetAMotorAxis(Amotor, i, 0, 0, 0, 1);
3057 i++;
3058 }
3059
3060 for (int j = 0; j < (int)axisnum; j++)
3061 {
3062 //d.JointSetAMotorAngle(Amotor, j, 0);
3063 }
3064
3065 //d.JointSetAMotorAngle(Amotor, 1, 0);
3066 //d.JointSetAMotorAngle(Amotor, 2, 0);
3067
3068 // These lowstops and high stops are effectively (no wiggle room)
3069 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0f);
3070 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f);
3071 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0f);
3072 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
3073 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
3074 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
3075 //d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 9000f);
3076 d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
3077 d.JointSetAMotorParam(Amotor, (int)dParam.FMax, Mass * 50f);//
3078
3079 }
3080
3081 public Matrix4 FromDMass(d.Mass pMass) 2635 public Matrix4 FromDMass(d.Mass pMass)
3082 { 2636 {
3083 Matrix4 obj; 2637 Matrix4 obj;
@@ -3124,7 +2678,7 @@ Console.WriteLine(" createAMotor called! ----------------------------");
3124 { 2678 {
3125 _parent_scene.remCollisionEventReporting(this); 2679 _parent_scene.remCollisionEventReporting(this);
3126 m_eventsubscription = 0; 2680 m_eventsubscription = 0;
3127 } 2681 }
3128 2682
3129 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) 2683 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
3130 { 2684 {
@@ -3326,5 +2880,1137 @@ Console.WriteLine(" createAMotor called! ----------------------------");
3326 m_material = pMaterial; 2880 m_material = pMaterial;
3327 } 2881 }
3328 2882
3329 } 2883
2884
2885
2886
2887
2888 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
2889 {
2890 switch (pParam)
2891 {
2892 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
2893 if (pValue < 0.01f) pValue = 0.01f;
2894 // m_angularDeflectionEfficiency = pValue;
2895 break;
2896 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
2897 if (pValue < 0.01f) pValue = 0.01f;
2898 // m_angularDeflectionTimescale = pValue;
2899 break;
2900 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
2901 if (pValue < 0.01f) pValue = 0.01f;
2902 m_angularMotorDecayTimescale = pValue;
2903 break;
2904 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
2905 if (pValue < 0.01f) pValue = 0.01f;
2906 m_angularMotorTimescale = pValue;
2907 break;
2908 case Vehicle.BANKING_EFFICIENCY:
2909 if (pValue < 0.01f) pValue = 0.01f;
2910 // m_bankingEfficiency = pValue;
2911 break;
2912 case Vehicle.BANKING_MIX:
2913 if (pValue < 0.01f) pValue = 0.01f;
2914 // m_bankingMix = pValue;
2915 break;
2916 case Vehicle.BANKING_TIMESCALE:
2917 if (pValue < 0.01f) pValue = 0.01f;
2918 // m_bankingTimescale = pValue;
2919 break;
2920 case Vehicle.BUOYANCY:
2921 if (pValue < -1f) pValue = -1f;
2922 if (pValue > 1f) pValue = 1f;
2923 m_VehicleBuoyancy = pValue;
2924 break;
2925// case Vehicle.HOVER_EFFICIENCY:
2926// if (pValue < 0f) pValue = 0f;
2927// if (pValue > 1f) pValue = 1f;
2928// m_VhoverEfficiency = pValue;
2929// break;
2930 case Vehicle.HOVER_HEIGHT:
2931 m_VhoverHeight = pValue;
2932 break;
2933 case Vehicle.HOVER_TIMESCALE:
2934 if (pValue < 0.01f) pValue = 0.01f;
2935 m_VhoverTimescale = pValue;
2936 break;
2937 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
2938 if (pValue < 0.01f) pValue = 0.01f;
2939 // m_linearDeflectionEfficiency = pValue;
2940 break;
2941 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
2942 if (pValue < 0.01f) pValue = 0.01f;
2943 // m_linearDeflectionTimescale = pValue;
2944 break;
2945 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
2946 if (pValue < 0.01f) pValue = 0.01f;
2947 m_linearMotorDecayTimescale = pValue;
2948 break;
2949 case Vehicle.LINEAR_MOTOR_TIMESCALE:
2950 if (pValue < 0.01f) pValue = 0.01f;
2951 m_linearMotorTimescale = pValue;
2952 break;
2953 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
2954 if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable
2955 if (pValue > 1.0f) pValue = 1.0f;
2956 m_verticalAttractionEfficiency = pValue;
2957 break;
2958 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
2959 if (pValue < 0.01f) pValue = 0.01f;
2960 m_verticalAttractionTimescale = pValue;
2961 break;
2962
2963 // These are vector properties but the engine lets you use a single float value to
2964 // set all of the components to the same value
2965 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
2966 if (pValue > 30f) pValue = 30f;
2967 if (pValue < 0.1f) pValue = 0.1f;
2968 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
2969 break;
2970 case Vehicle.ANGULAR_MOTOR_DIRECTION:
2971 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
2972 UpdateAngDecay();
2973 break;
2974 case Vehicle.LINEAR_FRICTION_TIMESCALE:
2975 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
2976 break;
2977 case Vehicle.LINEAR_MOTOR_DIRECTION:
2978 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
2979 UpdateLinDecay();
2980 break;
2981 case Vehicle.LINEAR_MOTOR_OFFSET:
2982 // m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
2983 break;
2984
2985 }
2986
2987 }//end ProcessFloatVehicleParam
2988
2989 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
2990 {
2991 switch (pParam)
2992 {
2993 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
2994 if (pValue.X > 30f) pValue.X = 30f;
2995 if (pValue.X < 0.1f) pValue.X = 0.1f;
2996 if (pValue.Y > 30f) pValue.Y = 30f;
2997 if (pValue.Y < 0.1f) pValue.Y = 0.1f;
2998 if (pValue.Z > 30f) pValue.Z = 30f;
2999 if (pValue.Z < 0.1f) pValue.Z = 0.1f;
3000 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
3001 break;
3002 case Vehicle.ANGULAR_MOTOR_DIRECTION:
3003 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
3004 // Limit requested angular speed to 2 rps= 4 pi rads/sec
3005 if(m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f;
3006 if(m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f;
3007 if(m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f;
3008 if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f;
3009 if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f;
3010 if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f;
3011 UpdateAngDecay();
3012 break;
3013 case Vehicle.LINEAR_FRICTION_TIMESCALE:
3014 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
3015 break;
3016 case Vehicle.LINEAR_MOTOR_DIRECTION:
3017 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); // velocity requested by LSL, for max limiting
3018 UpdateLinDecay();
3019 break;
3020 case Vehicle.LINEAR_MOTOR_OFFSET:
3021 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
3022 break;
3023 }
3024
3025 }//end ProcessVectorVehicleParam
3026
3027 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
3028 {
3029 switch (pParam)
3030 {
3031 case Vehicle.REFERENCE_FRAME:
3032 // m_referenceFrame = pValue;
3033 break;
3034 }
3035
3036 }//end ProcessRotationVehicleParam
3037
3038 internal void ProcessVehicleFlags(int pParam, bool remove)
3039 {
3040 if (remove)
3041 {
3042 m_flags &= ~((VehicleFlag)pParam);
3043 }
3044 else
3045 {
3046 m_flags |= (VehicleFlag)pParam;
3047 }
3048 }
3049
3050 internal void ProcessTypeChange(Vehicle pType)
3051 {
3052 // Set Defaults For Type
3053 m_type = pType;
3054 switch (pType)
3055 {
3056 case Vehicle.TYPE_SLED:
3057 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
3058 m_angularFrictionTimescale = new Vector3(30, 30, 30);
3059// m_lLinMotorVel = Vector3.Zero;
3060 m_linearMotorTimescale = 1000;
3061 m_linearMotorDecayTimescale = 120;
3062 m_angularMotorDirection = Vector3.Zero;
3063 m_angularMotorDVel = Vector3.Zero;
3064 m_angularMotorTimescale = 1000;
3065 m_angularMotorDecayTimescale = 120;
3066 m_VhoverHeight = 0;
3067// m_VhoverEfficiency = 1;
3068 m_VhoverTimescale = 10;
3069 m_VehicleBuoyancy = 0;
3070 // m_linearDeflectionEfficiency = 1;
3071 // m_linearDeflectionTimescale = 1;
3072 // m_angularDeflectionEfficiency = 1;
3073 // m_angularDeflectionTimescale = 1000;
3074 // m_bankingEfficiency = 0;
3075 // m_bankingMix = 1;
3076 // m_bankingTimescale = 10;
3077 // m_referenceFrame = Quaternion.Identity;
3078 m_flags &=
3079 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
3080 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
3081 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
3082 break;
3083 case Vehicle.TYPE_CAR:
3084 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
3085 m_angularFrictionTimescale = new Vector3(30, 30, 30); // was 1000, but sl max frict time is 30.
3086// m_lLinMotorVel = Vector3.Zero;
3087 m_linearMotorTimescale = 1;
3088 m_linearMotorDecayTimescale = 60;
3089 m_angularMotorDirection = Vector3.Zero;
3090 m_angularMotorDVel = Vector3.Zero;
3091 m_angularMotorTimescale = 1;
3092 m_angularMotorDecayTimescale = 0.8f;
3093 m_VhoverHeight = 0;
3094// m_VhoverEfficiency = 0;
3095 m_VhoverTimescale = 1000;
3096 m_VehicleBuoyancy = 0;
3097 // // m_linearDeflectionEfficiency = 1;
3098 // // m_linearDeflectionTimescale = 2;
3099 // // m_angularDeflectionEfficiency = 0;
3100 // m_angularDeflectionTimescale = 10;
3101 m_verticalAttractionEfficiency = 1f;
3102 m_verticalAttractionTimescale = 10f;
3103 // m_bankingEfficiency = -0.2f;
3104 // m_bankingMix = 1;
3105 // m_bankingTimescale = 1;
3106 // m_referenceFrame = Quaternion.Identity;
3107 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
3108 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY |
3109 VehicleFlag.LIMIT_MOTOR_UP);
3110 break;
3111 case Vehicle.TYPE_BOAT:
3112 m_linearFrictionTimescale = new Vector3(10, 3, 2);
3113 m_angularFrictionTimescale = new Vector3(10,10,10);
3114// m_lLinMotorVel = Vector3.Zero;
3115 m_linearMotorTimescale = 5;
3116 m_linearMotorDecayTimescale = 60;
3117 m_angularMotorDirection = Vector3.Zero;
3118 m_angularMotorDVel = Vector3.Zero;
3119 m_angularMotorTimescale = 4;
3120 m_angularMotorDecayTimescale = 4;
3121 m_VhoverHeight = 0;
3122// m_VhoverEfficiency = 0.5f;
3123 m_VhoverTimescale = 2;
3124 m_VehicleBuoyancy = 1;
3125 // m_linearDeflectionEfficiency = 0.5f;
3126 // m_linearDeflectionTimescale = 3;
3127 // m_angularDeflectionEfficiency = 0.5f;
3128 // m_angularDeflectionTimescale = 5;
3129 m_verticalAttractionEfficiency = 0.5f;
3130 m_verticalAttractionTimescale = 5f;
3131 // m_bankingEfficiency = -0.3f;
3132 // m_bankingMix = 0.8f;
3133 // m_bankingTimescale = 1;
3134 // m_referenceFrame = Quaternion.Identity;
3135 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY |
3136 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
3137 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY |
3138 VehicleFlag.LIMIT_MOTOR_UP);
3139 break;
3140 case Vehicle.TYPE_AIRPLANE:
3141 m_linearFrictionTimescale = new Vector3(200, 10, 5);
3142 m_angularFrictionTimescale = new Vector3(20, 20, 20);
3143// m_lLinMotorVel = Vector3.Zero;
3144 m_linearMotorTimescale = 2;
3145 m_linearMotorDecayTimescale = 60;
3146 m_angularMotorDirection = Vector3.Zero;
3147 m_angularMotorDVel = Vector3.Zero;
3148 m_angularMotorTimescale = 4;
3149 m_angularMotorDecayTimescale = 4;
3150 m_VhoverHeight = 0;
3151// m_VhoverEfficiency = 0.5f;
3152 m_VhoverTimescale = 1000;
3153 m_VehicleBuoyancy = 0;
3154 // m_linearDeflectionEfficiency = 0.5f;
3155 // m_linearDeflectionTimescale = 3;
3156 // m_angularDeflectionEfficiency = 1;
3157 // m_angularDeflectionTimescale = 2;
3158 m_verticalAttractionEfficiency = 0.9f;
3159 m_verticalAttractionTimescale = 2f;
3160 // m_bankingEfficiency = 1;
3161 // m_bankingMix = 0.7f;
3162 // m_bankingTimescale = 2;
3163 // m_referenceFrame = Quaternion.Identity;
3164 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
3165 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
3166 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
3167 break;
3168 case Vehicle.TYPE_BALLOON:
3169 m_linearFrictionTimescale = new Vector3(5, 5, 5);
3170 m_angularFrictionTimescale = new Vector3(10, 10, 10);
3171 m_linearMotorTimescale = 5;
3172 m_linearMotorDecayTimescale = 60;
3173 m_angularMotorDirection = Vector3.Zero;
3174 m_angularMotorDVel = Vector3.Zero;
3175 m_angularMotorTimescale = 6;
3176 m_angularMotorDecayTimescale = 10;
3177 m_VhoverHeight = 5;
3178// m_VhoverEfficiency = 0.8f;
3179 m_VhoverTimescale = 10;
3180 m_VehicleBuoyancy = 1;
3181 // m_linearDeflectionEfficiency = 0;
3182 // m_linearDeflectionTimescale = 5;
3183 // m_angularDeflectionEfficiency = 0;
3184 // m_angularDeflectionTimescale = 5;
3185 m_verticalAttractionEfficiency = 1f;
3186 m_verticalAttractionTimescale = 100f;
3187 // m_bankingEfficiency = 0;
3188 // m_bankingMix = 0.7f;
3189 // m_bankingTimescale = 5;
3190 // m_referenceFrame = Quaternion.Identity;
3191 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
3192 VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
3193 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
3194 break;
3195
3196 }
3197 }//end SetDefaultsForType
3198
3199 internal void Enable(IntPtr pBody, OdeScene pParentScene)
3200 {
3201 if (m_type == Vehicle.TYPE_NONE)
3202 return;
3203
3204 m_body = pBody;
3205 }
3206
3207
3208 internal void Halt()
3209 { // Kill all motions, when non-physical
3210 m_linearMotorDirection = Vector3.Zero;
3211 m_lLinMotorDVel = Vector3.Zero;
3212 m_lLinObjectVel = Vector3.Zero;
3213 m_wLinObjectVel = Vector3.Zero;
3214 m_angularMotorDirection = Vector3.Zero;
3215 m_lastAngularVelocity = Vector3.Zero;
3216 m_angularMotorDVel = Vector3.Zero;
3217 }
3218
3219 private void UpdateLinDecay()
3220 {
3221 if (Math.Abs(m_linearMotorDirection.X) > Math.Abs(m_lLinMotorDVel.X)) m_lLinMotorDVel.X = m_linearMotorDirection.X;
3222 if (Math.Abs(m_linearMotorDirection.Y) > Math.Abs(m_lLinMotorDVel.Y)) m_lLinMotorDVel.Y = m_linearMotorDirection.Y;
3223 if (Math.Abs(m_linearMotorDirection.Z) > Math.Abs(m_lLinMotorDVel.Z)) m_lLinMotorDVel.Z = m_linearMotorDirection.Z;
3224 } // else let the motor decay on its own
3225
3226 private void UpdateAngDecay()
3227 {
3228 if (Math.Abs(m_angularMotorDirection.X) > Math.Abs(m_angularMotorDVel.X)) m_angularMotorDVel.X = m_angularMotorDirection.X;
3229 if (Math.Abs(m_angularMotorDirection.Y) > Math.Abs(m_angularMotorDVel.Y)) m_angularMotorDVel.Y = m_angularMotorDirection.Y;
3230 if (Math.Abs(m_angularMotorDirection.Z) > Math.Abs(m_angularMotorDVel.Z)) m_angularMotorDVel.Z = m_angularMotorDirection.Z;
3231 } // else let the motor decay on its own
3232
3233 public void Move(float timestep)
3234 {
3235 float fx = 0;
3236 float fy = 0;
3237 float fz = 0;
3238
3239 frcount++; // used to limit debug comment output
3240 if (frcount > 100)
3241 frcount = 0;
3242
3243 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
3244 {
3245
3246// Old public void UpdatePositionAndVelocity(), more accuratley calculated here
3247 // Vector3 pv = Vector3.Zero; // what was this for?
3248 bool lastZeroFlag = _zeroFlag; // was it stopped
3249 // if (Body != (IntPtr)0) // FIXME -> or if it is a joint
3250 // {
3251 d.Vector3 vec = d.BodyGetPosition(Body);
3252 d.Quaternion ori = d.BodyGetQuaternion(Body);
3253 d.Vector3 vel = d.BodyGetLinearVel(Body);
3254 // d.Vector3 rotvel = d.BodyGetAngularVel(Body);
3255 d.Vector3 torque = d.BodyGetTorque(Body);
3256 _torque = new Vector3(torque.X, torque.Y, torque.Z);
3257 Vector3 l_position = Vector3.Zero;
3258 Quaternion l_orientation = Quaternion.Identity;
3259
3260 // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!)
3261 //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
3262 //if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
3263 //if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
3264 //if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
3265
3266 m_lastposition = _position;
3267 m_lastorientation = _orientation;
3268
3269 l_position.X = vec.X;
3270 l_position.Y = vec.Y;
3271 l_position.Z = vec.Z;
3272 l_orientation.X = ori.X;
3273 l_orientation.Y = ori.Y;
3274 l_orientation.Z = ori.Z;
3275 l_orientation.W = ori.W;
3276//Console.WriteLine("Move {0} at {1}", m_primName, l_position);
3277
3278
3279 if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) ||
3280 l_position.X < 0f ||
3281 l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) ||
3282 l_position.Y < 0f)
3283 {
3284 //base.RaiseOutOfBounds(l_position);
3285
3286 if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
3287 {
3288 _position = l_position;
3289 //_parent_scene.remActivePrim(this);
3290 if (_parent == null)
3291 base.RequestPhysicsterseUpdate();
3292 return;
3293 }
3294 else
3295 {
3296 if (_parent == null)
3297 base.RaiseOutOfBounds(l_position);
3298 return;
3299 }
3300 }
3301
3302 if (l_position.Z < 0)
3303 {
3304 // This is so prim that get lost underground don't fall forever and suck up
3305 //
3306 // Sim resources and memory.
3307 // Disables the prim's movement physics....
3308 // It's a hack and will generate a console message if it fails.
3309
3310 //IsPhysical = false;
3311 if (_parent == null)
3312 base.RaiseOutOfBounds(_position);
3313
3314 _acceleration.X = 0;
3315 _acceleration.Y = 0;
3316 _acceleration.Z = 0;
3317
3318 _velocity.X = 0;
3319 _velocity.Y = 0;
3320 _velocity.Z = 0;
3321 m_rotationalVelocity.X = 0;
3322 m_rotationalVelocity.Y = 0;
3323 m_rotationalVelocity.Z = 0;
3324
3325 if (_parent == null)
3326 base.RequestPhysicsterseUpdate();
3327
3328 m_throttleUpdates = false;
3329 throttleCounter = 0;
3330 _zeroFlag = true;
3331 //outofBounds = true;
3332 }
3333
3334 //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation));
3335//Console.WriteLine("Adiff " + m_primName + " = " + Adiff);
3336 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
3337 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
3338 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
3339// && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01))
3340 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large
3341 {
3342 _zeroFlag = true;
3343//Console.WriteLine("ZFT 2");
3344 m_throttleUpdates = false;
3345 }
3346 else
3347 {
3348 //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
3349 _zeroFlag = false;
3350 m_lastUpdateSent = false;
3351 //m_throttleUpdates = false;
3352 }
3353
3354 if (_zeroFlag)
3355 { // Its stopped
3356 _velocity.X = 0.0f;
3357 _velocity.Y = 0.0f;
3358 _velocity.Z = 0.0f;
3359
3360 _acceleration.X = 0;
3361 _acceleration.Y = 0;
3362 _acceleration.Z = 0;
3363
3364 //_orientation.w = 0f;
3365 //_orientation.X = 0f;
3366 //_orientation.Y = 0f;
3367 //_orientation.Z = 0f;
3368 m_rotationalVelocity.X = 0;
3369 m_rotationalVelocity.Y = 0;
3370 m_rotationalVelocity.Z = 0;
3371 if (!m_lastUpdateSent)
3372 {
3373 m_throttleUpdates = false;
3374 throttleCounter = 0;
3375 // m_rotationalVelocity = pv; What was this for?
3376
3377 if (_parent == null)
3378 {
3379 base.RequestPhysicsterseUpdate();
3380 }
3381
3382 m_lastUpdateSent = true;
3383 }
3384 }
3385 else
3386 { // Its moving
3387 if (lastZeroFlag != _zeroFlag)
3388 {
3389 if (_parent == null)
3390 {
3391 base.RequestPhysicsterseUpdate();
3392 }
3393 }
3394
3395 m_lastVelocity = _velocity;
3396
3397 _position = l_position;
3398
3399 _velocity.X = vel.X;
3400 _velocity.Y = vel.Y;
3401 _velocity.Z = vel.Z;
3402// Why 2 calcs???
3403// _acceleration = ((_velocity - m_lastVelocity) / 0.1f);
3404// _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f,
3405// _velocity.Y - m_lastVelocity.Y / 0.1f,
3406// _velocity.Z - m_lastVelocity.Z / 0.1f);
3407
3408 _acceleration = ((_velocity - m_lastVelocity) / timestep);
3409
3410 _orientation.X = ori.X;
3411 _orientation.Y = ori.Y;
3412 _orientation.Z = ori.Z;
3413 _orientation.W = ori.W;
3414 m_lastUpdateSent = false;
3415 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
3416 {
3417 if (_parent == null)
3418 {
3419 base.RequestPhysicsterseUpdate();
3420 }
3421 }
3422 else
3423 {
3424 throttleCounter++;
3425 }
3426 }
3427 m_lastposition = l_position;
3428
3429 /// End of old UpdatePositionAndVelocity insert
3430
3431//if (!Acceleration.ApproxEquals(Vector3.Zero, 0.01f)) Console.WriteLine("Move " + m_primName + " Accel=" + Acceleration);
3432//if(frcount == 0) Console.WriteLine("Move " + m_primName + " VTyp " + m_type +
3433 // " usePID=" + m_usePID + " seHover=" + m_useHoverPID + " useAPID=" + m_useAPID);
3434 if (m_type != Vehicle.TYPE_NONE)
3435 {
3436 // get body attitude
3437 d.Quaternion rot = d.BodyGetQuaternion(Body);
3438 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
3439 Quaternion irotq = Quaternion.Inverse(rotq);
3440
3441 // VEHICLE Linear Motion
3442 d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame
3443 Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z);
3444 m_lLinObjectVel = vel_now * irotq;
3445
3446 if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate
3447 {
3448 if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f)
3449 {
3450 float decayfactor = m_linearMotorDecayTimescale/timestep;
3451 Vector3 decayAmount = (m_lLinMotorDVel/decayfactor);
3452 m_lLinMotorDVel -= decayAmount;
3453 }
3454 else
3455 {
3456 float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale)));
3457 Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep;
3458 m_lLinMotorDVel -= decel;
3459 }
3460 if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
3461 {
3462 m_lLinMotorDVel = Vector3.Zero;
3463 }
3464 else
3465 {
3466 if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X;
3467 if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y;
3468 if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z;
3469 }
3470 } // end linear motor decay
3471
3472 if ( (! m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
3473 {
3474 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
3475 if (m_linearMotorTimescale < 300.0f)
3476 {
3477 Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel;
3478 float linfactor = m_linearMotorTimescale/timestep;
3479 Vector3 attackAmount = (attack_error/linfactor) * 1.3f;
3480 m_lLinObjectVel += attackAmount;
3481 }
3482 if (m_linearFrictionTimescale.X < 300.0f)
3483 {
3484 float fricfactor = m_linearFrictionTimescale.X / timestep;
3485 float fricX = m_lLinObjectVel.X / fricfactor;
3486 m_lLinObjectVel.X -= fricX;
3487 }
3488 if (m_linearFrictionTimescale.Y < 300.0f)
3489 {
3490 float fricfactor = m_linearFrictionTimescale.Y / timestep;
3491 float fricY = m_lLinObjectVel.Y / fricfactor;
3492 m_lLinObjectVel.Y -= fricY;
3493 }
3494 if (m_linearFrictionTimescale.Z < 300.0f)
3495 {
3496 float fricfactor = m_linearFrictionTimescale.Z / timestep;
3497//if(frcount == 0) Console.WriteLine("Zfric={0}", fricfactor);
3498 float fricZ = m_lLinObjectVel.Z / fricfactor;
3499 m_lLinObjectVel.Z -= fricZ;
3500 }
3501 }
3502 m_wLinObjectVel = m_lLinObjectVel * rotq;
3503
3504 // Gravity and Buoyancy
3505 Vector3 grav = Vector3.Zero;
3506 if(m_VehicleBuoyancy < 1.0f)
3507 {
3508 // There is some gravity, make a gravity force vector
3509 // that is applied after object velocity.
3510 d.Mass objMass;
3511 d.BodyGetMass(Body, out objMass);
3512 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
3513 grav.Z = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force
3514 } // else its 1.0, no gravity.
3515
3516 // Hovering
3517 if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
3518 {
3519 // We should hover, get the target height
3520 d.Vector3 pos = d.BodyGetPosition(Body);
3521 if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
3522 {
3523 m_VhoverTargetHeight = _parent_scene.GetWaterLevel() + m_VhoverHeight;
3524 }
3525 else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
3526 {
3527 m_VhoverTargetHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
3528 }
3529 else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
3530 {
3531 m_VhoverTargetHeight = m_VhoverHeight;
3532 }
3533
3534 if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
3535 {
3536 // If body is aready heigher, use its height as target height
3537 if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
3538 }
3539
3540// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
3541// m_VhoverTimescale = 0f; // time to acheive height
3542// timestep is time since last frame,in secs
3543 float herr0 = pos.Z - m_VhoverTargetHeight;
3544 // Replace Vertical speed with correction figure if significant
3545 if(Math.Abs(herr0) > 0.01f )
3546 {
3547 //? d.Mass objMass;
3548 //? d.BodyGetMass(Body, out objMass);
3549 m_wLinObjectVel.Z = - ( (herr0 * timestep * 50.0f) / m_VhoverTimescale);
3550 //KF: m_VhoverEfficiency is not yet implemented
3551 }
3552 else
3553 {
3554 m_wLinObjectVel.Z = 0f;
3555 }
3556 }
3557 else
3558 { // not hovering, Gravity rules
3559 m_wLinObjectVel.Z = vel_now.Z;
3560 }
3561
3562
3563 // Vehicle Linear Motion done =======================================
3564 // Apply velocity
3565 d.BodySetLinearVel(Body, m_wLinObjectVel.X, m_wLinObjectVel.Y, m_wLinObjectVel.Z);
3566 // apply gravity force
3567 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
3568//if(frcount == 0) Console.WriteLine("Grav {0}", grav);
3569 // end MoveLinear()
3570
3571
3572 // MoveAngular
3573 /*
3574 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
3575
3576 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
3577 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
3578 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
3579
3580 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
3581 private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body
3582 */
3583//if(frcount == 0) Console.WriteLine("MoveAngular ");
3584
3585 d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body);
3586 Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z);
3587 angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation
3588
3589//if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel);
3590
3591 // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack.
3592 float atk_decayfactor = 23.0f / (m_angularMotorTimescale * timestep);
3593 m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor;
3594 // Decay Angular Motor 2.
3595 if (m_angularMotorDecayTimescale < 300.0f)
3596 {
3597 if ( Vector3.Mag(m_angularMotorDVel) < 1.0f)
3598 {
3599 float decayfactor = (m_angularMotorDecayTimescale)/timestep;
3600 Vector3 decayAmount = (m_angularMotorDVel/decayfactor);
3601 m_angularMotorDVel -= decayAmount;
3602 }
3603 else
3604 {
3605 Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale;
3606 m_angularMotorDVel -= decel;
3607 }
3608
3609 if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
3610 {
3611 m_angularMotorDVel = Vector3.Zero;
3612 }
3613 else
3614 {
3615 if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X;
3616 if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y;
3617 if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z;
3618 }
3619 } // end decay angular motor
3620//if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel);
3621
3622//if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel);
3623
3624 // Vertical attractor section
3625 Vector3 vertattr = Vector3.Zero;
3626
3627 if(m_verticalAttractionTimescale < 300)
3628 {
3629 float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep);
3630 // make a vector pointing up
3631 Vector3 verterr = Vector3.Zero;
3632 verterr.Z = 1.0f;
3633 // rotate it to Body Angle
3634 verterr = verterr * rotq;
3635 // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
3636 // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go
3637 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
3638
3639 if (verterr.Z < 0.0f)
3640 { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to
3641 // vertical, BUT for some reason a z-rotation is imparted to the object. TBI.
3642//Console.WriteLine("InvertFlip");
3643 verterr.X = 2.0f - verterr.X;
3644 verterr.Y = 2.0f - verterr.Y;
3645 }
3646 verterr *= 0.5f;
3647 // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt)
3648
3649 if ((!angObjectVel.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f))
3650 {
3651 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
3652 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
3653 vertattr.X = verterr.Y;
3654 vertattr.Y = - verterr.X;
3655 vertattr.Z = 0f;
3656//if(frcount == 0) Console.WriteLine("VAerr=" + verterr);
3657
3658 // scaling appears better usingsquare-law
3659 float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
3660 float bounce = 1.0f - damped;
3661 // 0 = crit damp, 1 = bouncy
3662 float oavz = angObjectVel.Z; // retain z velocity
3663 // time-scaled correction, which sums, therefore is bouncy:
3664 angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce;
3665 // damped, good @ < 90:
3666 angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped);
3667 angObjectVel.Z = oavz;
3668//if(frcount == 0) Console.WriteLine("VA+");
3669//Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel);
3670 }
3671 else
3672 {
3673 // else error is very small
3674 angObjectVel.X = 0f;
3675 angObjectVel.Y = 0f;
3676//if(frcount == 0) Console.WriteLine("VA0");
3677 }
3678 } // else vertical attractor is off
3679//if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel);
3680
3681 if ( (! m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
3682 { // if motor or object have motion
3683 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
3684
3685 if (m_angularMotorTimescale < 300.0f)
3686 {
3687 Vector3 attack_error = m_angularMotorDVel - angObjectVel;
3688 float angfactor = m_angularMotorTimescale/timestep;
3689 Vector3 attackAmount = (attack_error/angfactor);
3690 angObjectVel += attackAmount;
3691//if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount);
3692//if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel);
3693 }
3694
3695 angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep);
3696 angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep);
3697 angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep);
3698 } // else no signif. motion
3699
3700//if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel);
3701 // Bank section tba
3702 // Deflection section tba
3703//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel);
3704
3705 m_lastAngularVelocity = angObjectVel;
3706//if(frcount == 0) Console.WriteLine("angularLock {0}", m_angularLock);
3707
3708 if (!m_angularLock.ApproxEquals(Vector3.One, 0.003f))
3709 {
3710 if (m_angularLock.X == 0)
3711 m_lastAngularVelocity.X = 0f;
3712 if (m_angularLock.Y == 0)
3713 m_lastAngularVelocity.Y = 0f;
3714 if (m_angularLock.Z == 0)
3715 m_lastAngularVelocity.Z = 0f;
3716 }
3717 // Apply to the body
3718// Vector3 aInc = m_lastAngularVelocity - initavel;
3719//if(frcount == 0) Console.WriteLine("Inc {0}", aInc);
3720 m_lastAngularVelocity = m_lastAngularVelocity * rotq; // ================ Converts to WORLD rotation
3721
3722 d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
3723//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity);
3724
3725 } // end VEHICLES ####
3726 else
3727 {
3728 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
3729 // NON-'VEHICLES' are dealt with here
3730 // m_angularlock = <1,1,1> means no lock. a 0 on axis means locked.
3731
3732// NB this may be wrong - may lock global axis! Should be LOCAL axis!
3733 /// Dynamics Angular Lock ========================================================================
3734 if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.One, 0.003f))
3735 {
3736 d.Vector3 avel2 = d.BodyGetAngularVel(Body);
3737 if (m_angularlock.X == 0)
3738 avel2.X = 0;
3739 if (m_angularlock.Y == 0)
3740 avel2.Y = 0;
3741 if (m_angularlock.Z == 0)
3742 avel2.Z = 0;
3743 d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
3744 }
3745
3746
3747 /// Dynamics Buoyancy ===============================================================================
3748 //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle.
3749 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
3750 // NB Prims in ODE are no subject to global gravity
3751 float m_mass = CalculateMass();
3752 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass
3753
3754 if (m_usePID)
3755 {
3756//if(frcount == 0) Console.WriteLine("PID " + m_primName);
3757 // KF - this is for object MoveToTarget.
3758
3759 //if (!d.BodyIsEnabled(Body))
3760 //d.BodySetForce(Body, 0f, 0f, 0f);
3761
3762 // no lock; for now it's only called from within Simulate()
3763
3764 // If the PID Controller isn't active then we set our force
3765 // calculating base velocity to the current position
3766
3767 if ((m_PIDTau < 1) && (m_PIDTau != 0))
3768 {
3769 //PID_G = PID_G / m_PIDTau;
3770 m_PIDTau = 1;
3771 }
3772
3773 if ((PID_G - m_PIDTau) <= 0)
3774 {
3775 PID_G = m_PIDTau + 1;
3776 }
3777 //PidStatus = true;
3778
3779 // PhysicsVector vec = new PhysicsVector();
3780// d.Vector3 vel = d.BodyGetLinearVel(Body);
3781
3782 d.Vector3 pos = d.BodyGetPosition(Body);
3783 _target_velocity =
3784 new Vector3(
3785 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
3786 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
3787 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
3788 );
3789
3790 // if velocity is zero, use position control; otherwise, velocity control
3791
3792 if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
3793 {
3794 // keep track of where we stopped. No more slippin' & slidin'
3795
3796 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3797 // react to the physics scene by moving it's position.
3798 // Avatar to Avatar collisions
3799 // Prim to avatar collisions
3800
3801 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
3802 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
3803 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
3804 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3805 d.BodySetLinearVel(Body, 0, 0, 0);
3806 d.BodyAddForce(Body, 0, 0, fz);
3807 return;
3808 }
3809 else
3810 {
3811 _zeroFlag = false;
3812
3813 // We're flying and colliding with something
3814 fx = ((_target_velocity.X) - vel.X) * (PID_D);
3815 fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
3816
3817 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
3818
3819 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
3820 }
3821 } // end if (m_usePID)
3822
3823 /// Dynamics Hover ===================================================================================
3824 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
3825 if (m_useHoverPID && !m_usePID)
3826 {
3827//Console.WriteLine("Hover " + m_primName);
3828
3829 // If we're using the PID controller, then we have no gravity
3830 fz = (-1 * _parent_scene.gravityz) * m_mass;
3831
3832 // no lock; for now it's only called from within Simulate()
3833
3834 // If the PID Controller isn't active then we set our force
3835 // calculating base velocity to the current position
3836
3837 if ((m_PIDTau < 1))
3838 {
3839 PID_G = PID_G / m_PIDTau;
3840 }
3841
3842 if ((PID_G - m_PIDTau) <= 0)
3843 {
3844 PID_G = m_PIDTau + 1;
3845 }
3846
3847
3848 // Where are we, and where are we headed?
3849 d.Vector3 pos = d.BodyGetPosition(Body);
3850// d.Vector3 vel = d.BodyGetLinearVel(Body);
3851
3852
3853 // Non-Vehicles have a limited set of Hover options.
3854 // determine what our target height really is based on HoverType
3855 switch (m_PIDHoverType)
3856 {
3857 case PIDHoverType.Ground:
3858 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3859 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3860 break;
3861 case PIDHoverType.GroundAndWater:
3862 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3863 m_waterHeight = _parent_scene.GetWaterLevel();
3864 if (m_groundHeight > m_waterHeight)
3865 {
3866 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3867 }
3868 else
3869 {
3870 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
3871 }
3872 break;
3873
3874 } // end switch (m_PIDHoverType)
3875
3876
3877 _target_velocity =
3878 new Vector3(0.0f, 0.0f,
3879 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
3880 );
3881
3882 // if velocity is zero, use position control; otherwise, velocity control
3883
3884 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
3885 {
3886 // keep track of where we stopped. No more slippin' & slidin'
3887
3888 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3889 // react to the physics scene by moving it's position.
3890 // Avatar to Avatar collisions
3891 // Prim to avatar collisions
3892
3893 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
3894 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
3895 d.BodyAddForce(Body, 0, 0, fz);
3896 //KF this prevents furthur motions return;
3897 }
3898 else
3899 {
3900 _zeroFlag = false;
3901
3902 // We're flying and colliding with something
3903 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
3904 }
3905 } // end m_useHoverPID && !m_usePID
3906
3907 /// Dynamics RotLookAt =================================================================================
3908 if (m_useAPID)
3909 {
3910 // RotLookAt, apparently overrides all other rotation sources. Inputs:
3911 // Quaternion m_APIDTarget
3912 // float m_APIDStrength // From SL experiments, this is the time to get there
3913 // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
3914 // Also in SL the mass of the object has no effect on time to get there.
3915 // Factors:
3916//if(frcount == 0) Console.WriteLine("APID ");
3917 // get present body rotation
3918 float limit = 1.0f;
3919 float scaler = 50f; // adjusts damping time
3920 float RLAservo = 0f;
3921
3922 d.Quaternion rot = d.BodyGetQuaternion(Body);
3923 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
3924 Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget;
3925 float diff_angle;
3926 Vector3 diff_axis;
3927 rot_diff.GetAxisAngle(out diff_axis, out diff_angle);
3928 diff_axis.Normalize();
3929 if(diff_angle > 0.01f) // diff_angle is always +ve
3930 {
3931// PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z);
3932 Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
3933 rotforce = rotforce * rotq;
3934 if(diff_angle > limit) diff_angle = limit; // cap the rotate rate
3935// RLAservo = timestep / m_APIDStrength * m_mass * scaler;
3936 // rotforce = rotforce * RLAservo * diff_angle ;
3937 // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z);
3938 RLAservo = timestep / m_APIDStrength * scaler;
3939 rotforce = rotforce * RLAservo * diff_angle ;
3940
3941 if (m_angularlock.X == 0)
3942 rotforce.X = 0;
3943 if (m_angularlock.Y == 0)
3944 rotforce.Y = 0;
3945 if (m_angularlock.Z == 0)
3946 rotforce.Z = 0;
3947
3948 d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z);
3949//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo);
3950 }
3951//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle);
3952 } // end m_useAPID
3953
3954 /// Dynamics Apply Forces ===================================================================================
3955 fx *= m_mass;
3956 fy *= m_mass;
3957 //fz *= m_mass;
3958
3959 fx += m_force.X;
3960 fy += m_force.Y;
3961 fz += m_force.Z;
3962
3963 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
3964 if (fx != 0 || fy != 0 || fz != 0)
3965 {
3966 //m_taintdisable = true;
3967 //base.RaiseOutOfBounds(Position);
3968 //d.BodySetLinearVel(Body, fx, fy, 0f);
3969 if (!d.BodyIsEnabled(Body))
3970 {
3971 // A physical body at rest on a surface will auto-disable after a while,
3972 // this appears to re-enable it incase the surface it is upon vanishes,
3973 // and the body should fall again.
3974 d.BodySetLinearVel(Body, 0f, 0f, 0f);
3975 d.BodySetForce(Body, 0, 0, 0);
3976 enableBodySoft();
3977 }
3978
3979 // 35x10 = 350n times the mass per second applied maximum.
3980 float nmax = 35f * m_mass;
3981 float nmin = -35f * m_mass;
3982
3983
3984 if (fx > nmax)
3985 fx = nmax;
3986 if (fx < nmin)
3987 fx = nmin;
3988 if (fy > nmax)
3989 fy = nmax;
3990 if (fy < nmin)
3991 fy = nmin;
3992 d.BodyAddForce(Body, fx, fy, fz);
3993//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
3994 }
3995 }
3996 }
3997 else
3998 { // is not physical, or is not a body or is selected
3999 // from old UpdatePositionAndVelocity, ... Not a body.. so Make sure the client isn't interpolating
4000 _velocity.X = 0;
4001 _velocity.Y = 0;
4002 _velocity.Z = 0;
4003
4004 _acceleration.X = 0;
4005 _acceleration.Y = 0;
4006 _acceleration.Z = 0;
4007
4008 m_rotationalVelocity.X = 0;
4009 m_rotationalVelocity.Y = 0;
4010 m_rotationalVelocity.Z = 0;
4011 _zeroFlag = true;
4012 return;
4013 }
4014 } // end Move()
4015 } // end class
3330} 4016}
generated by cgit v1.1 at 2024-12-15 05:27:29 +0000