aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs3875
1 files changed, 3875 insertions, 0 deletions
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
new file mode 100644
index 0000000..42e22ff
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -0,0 +1,3875 @@
1/* Copyright (c) Contributors, http://opensimulator.org/
2 * See CONTRIBUTORS.TXT for a full list of copyright holders.
3 * Redistribution and use in source and binary forms, with or without
4 * modification, are permitted provided that the following conditions are met:
5 * * Redistributions of source code must retain the above copyright
6 * notice, this list of conditions and the following disclaimer.
7 * * Redistributions in binary form must reproduce the above copyright
8 * notice, this list of conditions and the following disclaimer in the
9 * documentation and/or other materials provided with the distribution.
10 * * Neither the name of the OpenSimulator Project nor the
11 * names of its contributors may be used to endorse or promote products
12 * derived from this software without specific prior written permission.
13 *
14 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
15 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
16 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
17 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
18 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
19 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
20 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
21 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
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.
24 *
25 * Revised March 5th 2010 by Kitto Flora. ODEDynamics.cs
26 * rolled into ODEPrim.cs
27 */
28
29using System;
30using System.Collections.Generic;
31using System.Reflection;
32using System.Runtime.InteropServices;
33using System.Threading;
34using log4net;
35using OpenMetaverse;
36using Ode.NET;
37using OpenSim.Framework;
38using OpenSim.Region.Physics.Manager;
39
40
41namespace OpenSim.Region.Physics.OdePlugin
42{
43 /// <summary>
44 /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
45 /// </summary>
46
47 public class OdePrim : PhysicsActor
48 {
49 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
50
51 private Vector3 _position;
52 private Vector3 _velocity;
53 private Vector3 _torque;
54 private Vector3 m_lastVelocity;
55 private Vector3 m_lastposition;
56 private Quaternion m_lastorientation = new Quaternion();
57 private Vector3 m_rotationalVelocity;
58 private Vector3 _size;
59 private Vector3 _acceleration;
60 // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f);
61 private Quaternion _orientation;
62 private Vector3 m_taintposition;
63 private Vector3 m_taintsize;
64 private Vector3 m_taintVelocity;
65 private Vector3 m_taintTorque;
66 private Quaternion m_taintrot;
67 private Vector3 m_rotateEnable = Vector3.One; // Current setting
68 private Vector3 m_rotateEnableRequest = Vector3.One; // Request from LSL
69 private bool m_rotateEnableUpdate = false;
70 private Vector3 m_lockX;
71 private Vector3 m_lockY;
72 private Vector3 m_lockZ;
73 private IntPtr Amotor = IntPtr.Zero;
74 private IntPtr AmotorX = IntPtr.Zero;
75 private IntPtr AmotorY = IntPtr.Zero;
76 private IntPtr AmotorZ = IntPtr.Zero;
77
78 private Vector3 m_PIDTarget;
79 private float m_PIDTau;
80 private float PID_D = 35f;
81 private float PID_G = 25f;
82 private bool m_usePID = false;
83
84 private Quaternion m_APIDTarget = new Quaternion();
85 private float m_APIDStrength = 0.5f;
86 private float m_APIDDamping = 0.5f;
87 private bool m_useAPID = false;
88 private float m_APIDdamper = 1.0f;
89
90 // These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
91 // do not confuse with VEHICLE HOVER
92
93 private float m_PIDHoverHeight;
94 private float m_PIDHoverTau;
95 private bool m_useHoverPID;
96 private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
97 private float m_targetHoverHeight;
98 private float m_groundHeight;
99 private float m_waterHeight;
100 private float m_buoyancy; //m_buoyancy set by llSetBuoyancy()
101
102 // private float m_tensor = 5f;
103 private int body_autodisable_frames = 20;
104
105
106 private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
107 | CollisionCategories.Space
108 | CollisionCategories.Body
109 | CollisionCategories.Character
110 );
111 private bool m_taintshape;
112 private bool m_taintPhysics;
113 private bool m_collidesLand = true;
114 private bool m_collidesWater;
115 public bool m_returnCollisions;
116
117 // Default we're a Geometry
118 private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
119
120 // Default, Collide with Other Geometries, spaces and Bodies
121 private CollisionCategories m_collisionFlags = m_default_collisionFlags;
122
123 public bool m_taintremove;
124 public bool m_taintdisable;
125 public bool m_disabled;
126 public bool m_taintadd;
127 public bool m_taintselected;
128 public bool m_taintCollidesWater;
129
130 public uint m_localID;
131
132 //public GCHandle gc;
133 private CollisionLocker ode;
134
135 private bool m_meshfailed = false;
136 private bool m_taintforce = false;
137 private bool m_taintaddangularforce = false;
138 private Vector3 m_force;
139 private List<Vector3> m_forcelist = new List<Vector3>();
140 private List<Vector3> m_angularforcelist = new List<Vector3>();
141
142 private IMesh _mesh;
143 private PrimitiveBaseShape _pbs;
144 private OdeScene _parent_scene;
145 public IntPtr m_targetSpace = IntPtr.Zero;
146 public IntPtr prim_geom;
147// public IntPtr prev_geom;
148 public IntPtr _triMeshData;
149
150 private IntPtr _linkJointGroup = IntPtr.Zero;
151 private PhysicsActor _parent;
152 private PhysicsActor m_taintparent;
153
154 private List<OdePrim> childrenPrim = new List<OdePrim>();
155
156 private bool iscolliding;
157 private bool m_isphysical;
158 private bool m_isSelected;
159
160 internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
161
162 private bool m_throttleUpdates;
163 private int throttleCounter;
164 public int m_interpenetrationcount;
165 public float m_collisionscore;
166 public int m_roundsUnderMotionThreshold;
167 private int m_crossingfailures;
168
169 public bool outofBounds;
170 private float m_density = 10.000006836f; // Aluminum g/cm3;
171
172 public bool _zeroFlag; // if body has been stopped
173 private bool m_lastUpdateSent;
174
175 public IntPtr Body = IntPtr.Zero;
176 public String m_primName;
177 private Vector3 _target_velocity;
178 public d.Mass pMass;
179
180 public int m_eventsubscription;
181 private CollisionEventUpdate CollisionEventsThisFrame;
182
183 private IntPtr m_linkJoint = IntPtr.Zero;
184
185 public volatile bool childPrim;
186
187 internal int m_material = (int)Material.Wood;
188
189 private int frcount = 0; // Used to limit dynamics debug output to
190 private int revcount = 0; // Reverse motion while > 0
191
192 private IntPtr m_body = IntPtr.Zero;
193
194 // Vehicle properties ============================================================================================
195 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
196 // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
197 private VehicleFlag m_flags = (VehicleFlag) 0; // Bit settings:
198 // HOVER_TERRAIN_ONLY
199 // HOVER_GLOBAL_HEIGHT
200 // NO_DEFLECTION_UP
201 // HOVER_WATER_ONLY
202 // HOVER_UP_ONLY
203 // LIMIT_MOTOR_UP
204 // LIMIT_ROLL_ONLY
205
206 // Linear properties
207 private Vector3 m_linearMotorDirection = Vector3.Zero; // (was m_linearMotorDirectionLASTSET) the (local) Velocity
208 //requested by LSL
209 private float m_linearMotorTimescale = 0; // Motor Attack rate set by LSL
210 private float m_linearMotorDecayTimescale = 0; // Motor Decay rate set by LSL
211 private Vector3 m_linearFrictionTimescale = Vector3.Zero; // General Friction set by LSL
212
213 private Vector3 m_lLinMotorDVel = Vector3.Zero; // decayed motor
214 private Vector3 m_lLinObjectVel = Vector3.Zero; // local frame object velocity
215 private Vector3 m_wLinObjectVel = Vector3.Zero; // world frame object velocity
216
217 //Angular properties
218 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
219
220 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
221 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
222 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
223
224 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
225// private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity
226 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
227
228 //Deflection properties
229 // private float m_angularDeflectionEfficiency = 0;
230 // private float m_angularDeflectionTimescale = 0;
231 // private float m_linearDeflectionEfficiency = 0;
232 // private float m_linearDeflectionTimescale = 0;
233
234 //Banking properties
235 // private float m_bankingEfficiency = 0;
236 // private float m_bankingMix = 0;
237 // private float m_bankingTimescale = 0;
238
239 //Hover and Buoyancy properties
240 private float m_VhoverHeight = 0f;
241// private float m_VhoverEfficiency = 0f;
242 private float m_VhoverTimescale = 0f;
243 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
244 private float m_VehicleBuoyancy = 0f; // Set by VEHICLE_BUOYANCY, for a vehicle.
245 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
246 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
247 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
248
249 //Attractor properties
250 private float m_verticalAttractionEfficiency = 1.0f; // damped
251 private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor.
252
253
254
255
256
257
258 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
259 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode, uint localid)
260 {
261 m_localID = localid;
262 ode = dode;
263 if (!pos.IsFinite())
264 {
265 pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
266 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
267 m_log.Warn("[PHYSICS]: Got nonFinite Object create Position");
268 }
269
270 _position = pos;
271 m_taintposition = pos;
272 PID_D = parent_scene.bodyPIDD;
273 PID_G = parent_scene.bodyPIDG;
274 m_density = parent_scene.geomDefaultDensity;
275 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
276 body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
277
278
279 prim_geom = IntPtr.Zero;
280// prev_geom = IntPtr.Zero;
281
282 if (!pos.IsFinite())
283 {
284 size = new Vector3(0.5f, 0.5f, 0.5f);
285 m_log.Warn("[PHYSICS]: Got nonFinite Object create Size");
286 }
287
288 if (size.X <= 0) size.X = 0.01f;
289 if (size.Y <= 0) size.Y = 0.01f;
290 if (size.Z <= 0) size.Z = 0.01f;
291
292 _size = size;
293 m_taintsize = _size;
294
295 if (!QuaternionIsFinite(rotation))
296 {
297 rotation = Quaternion.Identity;
298 m_log.Warn("[PHYSICS]: Got nonFinite Object create Rotation");
299 }
300
301 _orientation = rotation;
302 m_taintrot = _orientation;
303 _mesh = mesh;
304 _pbs = pbs;
305
306 _parent_scene = parent_scene;
307 m_targetSpace = (IntPtr)0;
308
309// if (pos.Z < 0)
310 if (pos.Z < parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y))
311 m_isphysical = false;
312 else
313 {
314 m_isphysical = pisPhysical;
315 // If we're physical, we need to be in the master space for now.
316 // linksets *should* be in a space together.. but are not currently
317 if (m_isphysical)
318 m_targetSpace = _parent_scene.space;
319 }
320 m_primName = primName;
321 m_taintadd = true;
322 _parent_scene.AddPhysicsActorTaint(this);
323 // don't do .add() here; old geoms get recycled with the same hash
324 }
325
326 public override int PhysicsActorType
327 {
328 get { return (int) ActorTypes.Prim; }
329 set { return; }
330 }
331
332 public override bool SetAlwaysRun
333 {
334 get { return false; }
335 set { return; }
336 }
337
338 public override uint LocalID
339 {
340 set {
341 //m_log.Info("[PHYSICS]: Setting TrackerID: " + value);
342 m_localID = value; }
343 }
344
345 public override bool Grabbed
346 {
347 set { return; }
348 }
349
350 public override bool Selected
351 {
352 set {
353
354//Console.WriteLine("Sel {0} {1} {2}", m_primName, value, m_isphysical);
355 // This only makes the object not collidable if the object
356 // is physical or the object is modified somehow *IN THE FUTURE*
357 // without this, if an avatar selects prim, they can walk right
358 // through it while it's selected
359 m_collisionscore = 0;
360 if ((m_isphysical && !_zeroFlag) || !value)
361 {
362 m_taintselected = value;
363 _parent_scene.AddPhysicsActorTaint(this);
364 }
365 else
366 {
367 m_taintselected = value;
368 m_isSelected = value;
369 }
370 if(m_isSelected) disableBodySoft();
371 }
372 }
373
374 public override bool IsPhysical
375 {
376 get { return m_isphysical; }
377 set
378 {
379 m_isphysical = value;
380 if (!m_isphysical)
381 { // Zero the remembered last velocity
382 m_lastVelocity = Vector3.Zero;
383 if (m_type != Vehicle.TYPE_NONE) Halt();
384 }
385 }
386 }
387
388 public void setPrimForRemoval()
389 {
390 m_taintremove = true;
391 }
392
393 public override bool Flying
394 {
395 // no flying prims for you
396 get { return false; }
397 set { }
398 }
399
400 public override bool IsColliding
401 {
402 get { return iscolliding; }
403 set { iscolliding = value; }
404 }
405
406 public override bool CollidingGround
407 {
408 get { return false; }
409 set { return; }
410 }
411
412 public override bool CollidingObj
413 {
414 get { return false; }
415 set { return; }
416 }
417
418 public override bool ThrottleUpdates
419 {
420 get { return m_throttleUpdates; }
421 set { m_throttleUpdates = value; }
422 }
423
424 public override bool Stopped
425 {
426 get { return _zeroFlag; }
427 }
428
429 public override Vector3 Position
430 {
431 get { return _position; }
432
433 set { _position = value;
434 //m_log.Info("[PHYSICS]: " + _position.ToString());
435 }
436 }
437
438 public override Vector3 Size
439 {
440 get { return _size; }
441 set
442 {
443 if (value.IsFinite())
444 {
445 _size = value;
446 }
447 else
448 {
449 m_log.Warn("[PHYSICS]: Got NaN Size on object");
450 }
451 }
452 }
453
454 public override float Mass
455 {
456 get { return CalculateMass(); }
457 }
458
459 public override Vector3 Force
460 {
461 //get { return Vector3.Zero; }
462 get { return m_force; }
463 set
464 {
465 if (value.IsFinite())
466 {
467 m_force = value;
468 }
469 else
470 {
471 m_log.Warn("[PHYSICS]: NaN in Force Applied to an Object");
472 }
473 }
474 }
475
476 public override int VehicleType
477 {
478 get { return (int)m_type; }
479 set { ProcessTypeChange((Vehicle)value); }
480 }
481
482 public override void VehicleFloatParam(int param, float value)
483 {
484 ProcessFloatVehicleParam((Vehicle) param, value);
485 }
486
487 public override void VehicleVectorParam(int param, Vector3 value)
488 {
489 ProcessVectorVehicleParam((Vehicle) param, value);
490 }
491
492 public override void VehicleRotationParam(int param, Quaternion rotation)
493 {
494 ProcessRotationVehicleParam((Vehicle) param, rotation);
495 }
496
497 public override void VehicleFlags(int param, bool remove)
498 {
499 ProcessVehicleFlags(param, remove);
500 }
501
502 public override void SetVolumeDetect(int param)
503 {
504 lock (_parent_scene.OdeLock)
505 {
506 m_isVolumeDetect = (param!=0);
507 }
508 }
509
510 public override Vector3 CenterOfMass
511 {
512 get { return Vector3.Zero; }
513 }
514
515 public override Vector3 GeometricCenter
516 {
517 get { return Vector3.Zero; }
518 }
519
520 public override PrimitiveBaseShape Shape
521 {
522 set
523 {
524 _pbs = value;
525 m_taintshape = true;
526 }
527 }
528
529 public override Vector3 Velocity
530 {
531 get
532 {
533 // Averate previous velocity with the new one so
534 // client object interpolation works a 'little' better
535 if (_zeroFlag)
536 return Vector3.Zero;
537
538 Vector3 returnVelocity = Vector3.Zero;
539 returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2;
540 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2;
541 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2;
542 return returnVelocity;
543 }
544 set
545 {
546 if (value.IsFinite())
547 {
548 _velocity = value;
549 if (_velocity.ApproxEquals(Vector3.Zero,0.001f))
550 _acceleration = Vector3.Zero;
551
552 m_taintVelocity = value;
553 _parent_scene.AddPhysicsActorTaint(this);
554 }
555 else
556 {
557 m_log.Warn("[PHYSICS]: Got NaN Velocity in Object");
558 }
559
560 }
561 }
562
563 public override Vector3 Torque
564 {
565 get
566 {
567 if (!m_isphysical || Body == IntPtr.Zero)
568 return Vector3.Zero;
569
570 return _torque;
571 }
572
573 set
574 {
575 if (value.IsFinite())
576 {
577 m_taintTorque = value;
578 _parent_scene.AddPhysicsActorTaint(this);
579 }
580 else
581 {
582 m_log.Warn("[PHYSICS]: Got NaN Torque in Object");
583 }
584 }
585 }
586
587 public override float CollisionScore
588 {
589 get { return m_collisionscore; }
590 set { m_collisionscore = value; }
591 }
592
593 public override bool Kinematic
594 {
595 get { return false; }
596 set { }
597 }
598
599 public override Quaternion Orientation
600 {
601 get { return _orientation; }
602 set
603 {
604 if (QuaternionIsFinite(value))
605 {
606 _orientation = value;
607 }
608 else
609 m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object");
610
611 }
612 }
613
614
615 public override bool FloatOnWater
616 {
617 set {
618 m_taintCollidesWater = value;
619 _parent_scene.AddPhysicsActorTaint(this);
620 }
621 }
622
623 public override void SetMomentum(Vector3 momentum)
624 {
625 }
626
627 public override Vector3 PIDTarget
628 {
629 set
630 {
631 if (value.IsFinite())
632 {
633 m_PIDTarget = value;
634 }
635 else
636 m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object");
637 }
638 }
639 public override bool PIDActive { set { m_usePID = value; } }
640 public override float PIDTau { set { m_PIDTau = value; } }
641
642 // For RotLookAt
643 public override Quaternion APIDTarget { set { m_APIDTarget = value; } }
644 public override bool APIDActive { set { m_useAPID = value; } }
645 public override float APIDStrength { set { m_APIDStrength = value; } }
646 public override float APIDDamping { set { m_APIDDamping = value; } }
647
648 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
649 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
650 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
651 public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
652
653 internal static bool QuaternionIsFinite(Quaternion q)
654 {
655 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
656 return false;
657 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
658 return false;
659 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
660 return false;
661 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
662 return false;
663 return true;
664 }
665
666 public override Vector3 Acceleration // client updates read data via here
667 {
668 get
669 {
670 if (_zeroFlag)
671 {
672 return Vector3.Zero;
673 }
674 return _acceleration;
675 }
676 set { _acceleration = value; }
677 }
678
679
680 public void SetAcceleration(Vector3 accel) // No one calls this, and it would not do anything.
681 {
682 _acceleration = accel;
683 }
684
685 public override void AddForce(Vector3 force, bool pushforce)
686 {
687 if (force.IsFinite())
688 {
689 lock (m_forcelist)
690 m_forcelist.Add(force);
691
692 m_taintforce = true;
693 }
694 else
695 {
696 m_log.Warn("[PHYSICS]: Got Invalid linear force vector from Scene in Object");
697 }
698 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
699 }
700
701 public override void AddAngularForce(Vector3 force, bool pushforce)
702 {
703 if (force.IsFinite())
704 {
705 m_angularforcelist.Add(force);
706 m_taintaddangularforce = true;
707 }
708 else
709 {
710 m_log.Warn("[PHYSICS]: Got Invalid Angular force vector from Scene in Object");
711 }
712 }
713
714 public override Vector3 RotationalVelocity
715 {
716 get
717 {
718 return m_rotationalVelocity;
719 }
720 set
721 {
722 if (value.IsFinite())
723 {
724 m_rotationalVelocity = value;
725 }
726 else
727 {
728 m_log.Warn("[PHYSICS]: Got NaN RotationalVelocity in Object");
729 }
730 }
731 }
732
733 public override void CrossingFailure()
734 {
735 m_crossingfailures++;
736 if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
737 {
738 base.RaiseOutOfBounds(_position);
739 return;
740 }
741 else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
742 {
743 m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);
744 }
745 }
746
747 public override float Buoyancy
748 {
749 get { return m_buoyancy; }
750 set { m_buoyancy = value; }
751 }
752
753 public override void link(PhysicsActor obj)
754 {
755 m_taintparent = obj;
756 }
757
758 public override void delink()
759 {
760 m_taintparent = null;
761 }
762
763 public override void LockAngularMotion(Vector3 axis)
764 {
765 // This is actually ROTATION ENABLE, not a lock.
766 // default is <1,1,1> which is all enabled.
767 // The lock value is updated inside Move(), no point in using the taint system.
768 // OS 'm_taintAngularLock' etc change to m_rotateEnable.
769 if (axis.IsFinite())
770 {
771 axis.X = (axis.X > 0) ? 1f : 0f;
772 axis.Y = (axis.Y > 0) ? 1f : 0f;
773 axis.Z = (axis.Z > 0) ? 1f : 0f;
774 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
775 m_rotateEnableRequest = axis;
776 m_rotateEnableUpdate = true;
777 }
778 else
779 {
780 m_log.Warn("[PHYSICS]: Got NaN locking axis from Scene on Object");
781 }
782 }
783
784
785 public void SetGeom(IntPtr geom)
786 {
787 if(prim_geom != IntPtr.Zero)
788 {
789 // Remove any old entries
790//string tPA;
791//_parent_scene.geom_name_map.TryGetValue(prim_geom, out tPA);
792//Console.WriteLine("**** Remove {0}", tPA);
793 if(_parent_scene.geom_name_map.ContainsKey(prim_geom)) _parent_scene.geom_name_map.Remove(prim_geom);
794 if(_parent_scene.actor_name_map.ContainsKey(prim_geom)) _parent_scene.actor_name_map.Remove(prim_geom);
795 d.GeomDestroy(prim_geom);
796 }
797
798 prim_geom = geom;
799//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName);
800 if (prim_geom != IntPtr.Zero)
801 {
802 _parent_scene.geom_name_map[prim_geom] = this.m_primName;
803 _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
804 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
805 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
806//Console.WriteLine("**** Create {2} Dicts: actor={0} name={1}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, this.m_primName);
807 }
808
809 if (childPrim)
810 {
811 if (_parent != null && _parent is OdePrim)
812 {
813 OdePrim parent = (OdePrim)_parent;
814//Console.WriteLine("SetGeom calls ChildSetGeom");
815 parent.ChildSetGeom(this);
816 }
817 }
818 //m_log.Warn("Setting Geom to: " + prim_geom);
819 }
820
821 public void enableBodySoft()
822 {
823 if (!childPrim)
824 {
825 if (m_isphysical && Body != IntPtr.Zero)
826 {
827 d.BodyEnable(Body);
828 if (m_type != Vehicle.TYPE_NONE)
829 Enable(Body, _parent_scene);
830 }
831
832 m_disabled = false;
833 }
834 }
835
836 public void disableBodySoft()
837 {
838 m_disabled = true;
839
840 if (m_isphysical && Body != IntPtr.Zero)
841 {
842 d.BodyDisable(Body);
843 Halt();
844 }
845 }
846
847 public void enableBody()
848 {
849 // Don't enable this body if we're a child prim
850 // this should be taken care of in the parent function not here
851 if (!childPrim)
852 {
853 // Sets the geom to a body
854 Body = d.BodyCreate(_parent_scene.world);
855
856 setMass();
857 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
858 d.Quaternion myrot = new d.Quaternion();
859 myrot.X = _orientation.X;
860 myrot.Y = _orientation.Y;
861 myrot.Z = _orientation.Z;
862 myrot.W = _orientation.W;
863 d.BodySetQuaternion(Body, ref myrot);
864 d.GeomSetBody(prim_geom, Body);
865 m_collisionCategories |= CollisionCategories.Body;
866 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
867
868 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
869 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
870
871 d.BodySetAutoDisableFlag(Body, true);
872 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
873
874 // disconnect from world gravity so we can apply buoyancy
875 d.BodySetGravityMode (Body, false);
876
877 m_interpenetrationcount = 0;
878 m_collisionscore = 0;
879 m_disabled = false;
880
881 if (m_type != Vehicle.TYPE_NONE)
882 {
883 Enable(Body, _parent_scene);
884 }
885
886 _parent_scene.addActivePrim(this);
887 }
888 }
889
890 #region Mass Calculation
891
892 private float CalculateMass()
893 {
894 float volume = _size.X * _size.Y * _size.Z; // default
895 float tmp;
896
897 float returnMass = 0;
898 float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
899 float hollowVolume = hollowAmount * hollowAmount;
900
901 switch (_pbs.ProfileShape)
902 {
903 case ProfileShape.Square:
904 // default box
905
906 if (_pbs.PathCurve == (byte)Extrusion.Straight)
907 {
908 if (hollowAmount > 0.0)
909 {
910 switch (_pbs.HollowShape)
911 {
912 case HollowShape.Square:
913 case HollowShape.Same:
914 break;
915
916 case HollowShape.Circle:
917
918 hollowVolume *= 0.78539816339f;
919 break;
920
921 case HollowShape.Triangle:
922
923 hollowVolume *= (0.5f * .5f);
924 break;
925
926 default:
927 hollowVolume = 0;
928 break;
929 }
930 volume *= (1.0f - hollowVolume);
931 }
932 }
933
934 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
935 {
936 //a tube
937
938 volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
939 tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY);
940 volume -= volume*tmp*tmp;
941
942 if (hollowAmount > 0.0)
943 {
944 hollowVolume *= hollowAmount;
945
946 switch (_pbs.HollowShape)
947 {
948 case HollowShape.Square:
949 case HollowShape.Same:
950 break;
951
952 case HollowShape.Circle:
953 hollowVolume *= 0.78539816339f;;
954 break;
955
956 case HollowShape.Triangle:
957 hollowVolume *= 0.5f * 0.5f;
958 break;
959 default:
960 hollowVolume = 0;
961 break;
962 }
963 volume *= (1.0f - hollowVolume);
964 }
965 }
966
967 break;
968
969 case ProfileShape.Circle:
970
971 if (_pbs.PathCurve == (byte)Extrusion.Straight)
972 {
973 volume *= 0.78539816339f; // elipse base
974
975 if (hollowAmount > 0.0)
976 {
977 switch (_pbs.HollowShape)
978 {
979 case HollowShape.Same:
980 case HollowShape.Circle:
981 break;
982
983 case HollowShape.Square:
984 hollowVolume *= 0.5f * 2.5984480504799f;
985 break;
986
987 case HollowShape.Triangle:
988 hollowVolume *= .5f * 1.27323954473516f;
989 break;
990
991 default:
992 hollowVolume = 0;
993 break;
994 }
995 volume *= (1.0f - hollowVolume);
996 }
997 }
998
999 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1000 {
1001 volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
1002 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
1003 volume *= (1.0f - tmp * tmp);
1004
1005 if (hollowAmount > 0.0)
1006 {
1007
1008 // calculate the hollow volume by it's shape compared to the prim shape
1009 hollowVolume *= hollowAmount;
1010
1011 switch (_pbs.HollowShape)
1012 {
1013 case HollowShape.Same:
1014 case HollowShape.Circle:
1015 break;
1016
1017 case HollowShape.Square:
1018 hollowVolume *= 0.5f * 2.5984480504799f;
1019 break;
1020
1021 case HollowShape.Triangle:
1022 hollowVolume *= .5f * 1.27323954473516f;
1023 break;
1024
1025 default:
1026 hollowVolume = 0;
1027 break;
1028 }
1029 volume *= (1.0f - hollowVolume);
1030 }
1031 }
1032 break;
1033
1034 case ProfileShape.HalfCircle:
1035 if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1036 {
1037 volume *= 0.52359877559829887307710723054658f;
1038 }
1039 break;
1040
1041 case ProfileShape.EquilateralTriangle:
1042
1043 if (_pbs.PathCurve == (byte)Extrusion.Straight)
1044 {
1045 volume *= 0.32475953f;
1046
1047 if (hollowAmount > 0.0)
1048 {
1049
1050 // calculate the hollow volume by it's shape compared to the prim shape
1051 switch (_pbs.HollowShape)
1052 {
1053 case HollowShape.Same:
1054 case HollowShape.Triangle:
1055 hollowVolume *= .25f;
1056 break;
1057
1058 case HollowShape.Square:
1059 hollowVolume *= 0.499849f * 3.07920140172638f;
1060 break;
1061
1062 case HollowShape.Circle:
1063 // Hollow shape is a perfect cyllinder in respect to the cube's scale
1064 // Cyllinder hollow volume calculation
1065
1066 hollowVolume *= 0.1963495f * 3.07920140172638f;
1067 break;
1068
1069 default:
1070 hollowVolume = 0;
1071 break;
1072 }
1073 volume *= (1.0f - hollowVolume);
1074 }
1075 }
1076 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1077 {
1078 volume *= 0.32475953f;
1079 volume *= 0.01f * (float)(200 - _pbs.PathScaleX);
1080 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
1081 volume *= (1.0f - tmp * tmp);
1082
1083 if (hollowAmount > 0.0)
1084 {
1085
1086 hollowVolume *= hollowAmount;
1087
1088 switch (_pbs.HollowShape)
1089 {
1090 case HollowShape.Same:
1091 case HollowShape.Triangle:
1092 hollowVolume *= .25f;
1093 break;
1094
1095 case HollowShape.Square:
1096 hollowVolume *= 0.499849f * 3.07920140172638f;
1097 break;
1098
1099 case HollowShape.Circle:
1100
1101 hollowVolume *= 0.1963495f * 3.07920140172638f;
1102 break;
1103
1104 default:
1105 hollowVolume = 0;
1106 break;
1107 }
1108 volume *= (1.0f - hollowVolume);
1109 }
1110 }
1111 break;
1112
1113 default:
1114 break;
1115 }
1116
1117
1118
1119 float taperX1;
1120 float taperY1;
1121 float taperX;
1122 float taperY;
1123 float pathBegin;
1124 float pathEnd;
1125 float profileBegin;
1126 float profileEnd;
1127
1128 if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible)
1129 {
1130 taperX1 = _pbs.PathScaleX * 0.01f;
1131 if (taperX1 > 1.0f)
1132 taperX1 = 2.0f - taperX1;
1133 taperX = 1.0f - taperX1;
1134
1135 taperY1 = _pbs.PathScaleY * 0.01f;
1136 if (taperY1 > 1.0f)
1137 taperY1 = 2.0f - taperY1;
1138 taperY = 1.0f - taperY1;
1139 }
1140 else
1141 {
1142 taperX = _pbs.PathTaperX * 0.01f;
1143 if (taperX < 0.0f)
1144 taperX = -taperX;
1145 taperX1 = 1.0f - taperX;
1146
1147 taperY = _pbs.PathTaperY * 0.01f;
1148 if (taperY < 0.0f)
1149 taperY = -taperY;
1150 taperY1 = 1.0f - taperY;
1151
1152 }
1153
1154
1155 volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY);
1156
1157 pathBegin = (float)_pbs.PathBegin * 2.0e-5f;
1158 pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f;
1159 volume *= (pathEnd - pathBegin);
1160
1161// this is crude aproximation
1162 profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f;
1163 profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f;
1164 volume *= (profileEnd - profileBegin);
1165
1166 returnMass = m_density * volume;
1167
1168 if (returnMass <= 0)
1169 returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
1170// else if (returnMass > _parent_scene.maximumMassObject)
1171// returnMass = _parent_scene.maximumMassObject;
1172
1173
1174
1175
1176 // Recursively calculate mass
1177 bool HasChildPrim = false;
1178 lock (childrenPrim)
1179 {
1180 if (childrenPrim.Count > 0)
1181 {
1182 HasChildPrim = true;
1183 }
1184
1185 }
1186 if (HasChildPrim)
1187 {
1188 OdePrim[] childPrimArr = new OdePrim[0];
1189
1190 lock (childrenPrim)
1191 childPrimArr = childrenPrim.ToArray();
1192
1193 for (int i = 0; i < childPrimArr.Length; i++)
1194 {
1195 if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove)
1196 returnMass += childPrimArr[i].CalculateMass();
1197 // failsafe, this shouldn't happen but with OpenSim, you never know :)
1198 if (i > 256)
1199 break;
1200 }
1201 }
1202 if (returnMass > _parent_scene.maximumMassObject)
1203 returnMass = _parent_scene.maximumMassObject;
1204 return returnMass;
1205 }// end CalculateMass
1206
1207 #endregion
1208
1209 public void setMass()
1210 {
1211 if (Body != (IntPtr) 0)
1212 {
1213 float newmass = CalculateMass();
1214
1215 //m_log.Info("[PHYSICS]: New Mass: " + newmass.ToString());
1216
1217 d.MassSetBoxTotal(out pMass, newmass, _size.X, _size.Y, _size.Z);
1218 d.BodySetMass(Body, ref pMass);
1219 }
1220 }
1221
1222 public void disableBody()
1223 {
1224 //this kills the body so things like 'mesh' can re-create it.
1225 lock (this)
1226 {
1227 if (!childPrim)
1228 {
1229 if (Body != IntPtr.Zero)
1230 {
1231 _parent_scene.remActivePrim(this);
1232 m_collisionCategories &= ~CollisionCategories.Body;
1233 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
1234
1235 if (prim_geom != IntPtr.Zero)
1236 {
1237 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1238 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1239 }
1240
1241
1242 d.BodyDestroy(Body);
1243 lock (childrenPrim)
1244 {
1245 if (childrenPrim.Count > 0)
1246 {
1247 foreach (OdePrim prm in childrenPrim)
1248 {
1249 _parent_scene.remActivePrim(prm);
1250 prm.Body = IntPtr.Zero;
1251 }
1252 }
1253 }
1254 Body = IntPtr.Zero;
1255 }
1256 }
1257 else
1258 {
1259 _parent_scene.remActivePrim(this);
1260
1261 m_collisionCategories &= ~CollisionCategories.Body;
1262 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
1263
1264 if (prim_geom != IntPtr.Zero)
1265 {
1266 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1267 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1268 }
1269
1270
1271 Body = IntPtr.Zero;
1272 }
1273 }
1274 m_disabled = true;
1275 m_collisionscore = 0;
1276 }
1277
1278 private static Dictionary<IMesh, IntPtr> m_MeshToTriMeshMap = new Dictionary<IMesh, IntPtr>();
1279
1280 public void setMesh(OdeScene parent_scene, IMesh mesh)
1281 {
1282 // This sleeper is there to moderate how long it takes between
1283 // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object
1284
1285 //Thread.Sleep(10);
1286
1287 //Kill Body so that mesh can re-make the geom
1288 if (IsPhysical && Body != IntPtr.Zero)
1289 {
1290 if (childPrim)
1291 {
1292 if (_parent != null)
1293 {
1294 OdePrim parent = (OdePrim)_parent;
1295 parent.ChildDelink(this);
1296 }
1297 }
1298 else
1299 {
1300 disableBody();
1301 }
1302 }
1303 IntPtr vertices, indices;
1304 int vertexCount, indexCount;
1305 int vertexStride, triStride;
1306 mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
1307 mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
1308
1309 mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
1310 if (m_MeshToTriMeshMap.ContainsKey(mesh))
1311 {
1312 _triMeshData = m_MeshToTriMeshMap[mesh];
1313 }
1314 else
1315 {
1316 _triMeshData = d.GeomTriMeshDataCreate();
1317
1318 d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
1319 d.GeomTriMeshDataPreprocess(_triMeshData);
1320 m_MeshToTriMeshMap[mesh] = _triMeshData;
1321 }
1322
1323 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1324 try
1325 {
1326 // if (prim_geom == IntPtr.Zero) // setGeom takes care of phys engine recreate and prim_geom pointer
1327 // {
1328 SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
1329 // }
1330 }
1331 catch (AccessViolationException)
1332 {
1333 m_log.Error("[PHYSICS]: MESH LOCKED");
1334 return;
1335 }
1336
1337
1338 // if (IsPhysical && Body == (IntPtr) 0)
1339 // {
1340 // Recreate the body
1341 // m_interpenetrationcount = 0;
1342 // m_collisionscore = 0;
1343
1344 // enableBody();
1345 // }
1346 }
1347
1348 public void ProcessTaints(float timestep) //=============================================================================
1349 {
1350 if (m_taintadd)
1351 {
1352 changeadd(timestep);
1353 }
1354
1355 if (prim_geom != IntPtr.Zero)
1356 {
1357 if (!_position.ApproxEquals(m_taintposition, 0f))
1358 {
1359 changemove(timestep);
1360 }
1361 if (m_taintrot != _orientation)
1362 {
1363 if(childPrim && IsPhysical) // For physical child prim...
1364 {
1365 rotate(timestep);
1366 // KF: ODE will also rotate the parent prim!
1367 // so rotate the root back to where it was
1368 OdePrim parent = (OdePrim)_parent;
1369 parent.rotate(timestep);
1370 }
1371 else
1372 {
1373 //Just rotate the prim
1374 rotate(timestep);
1375 }
1376 }
1377 //
1378
1379 if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent))
1380 {
1381 changePhysicsStatus(timestep);
1382 }//
1383
1384 if (!_size.ApproxEquals(m_taintsize,0f))
1385 changesize(timestep);
1386 //
1387
1388 if (m_taintshape)
1389 changeshape(timestep);
1390 //
1391
1392 if (m_taintforce)
1393 changeAddForce(timestep);
1394
1395 if (m_taintaddangularforce)
1396 changeAddAngularForce(timestep);
1397
1398 if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f))
1399 changeSetTorque(timestep);
1400
1401 if (m_taintdisable)
1402 changedisable(timestep);
1403
1404 if (m_taintselected != m_isSelected)
1405 changeSelectedStatus(timestep);
1406
1407 if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f))
1408 changevelocity(timestep);
1409
1410 if (m_taintparent != _parent)
1411 changelink(timestep);
1412
1413 if (m_taintCollidesWater != m_collidesWater)
1414 changefloatonwater(timestep);
1415/* obsolete
1416 if (!m_angularLock.ApproxEquals(m_taintAngularLock,0f))
1417 changeAngularLock(timestep);
1418 */
1419 }
1420 else
1421 {
1422 m_log.Error("[PHYSICS]: The scene reused a disposed PhysActor! *waves finger*, Don't be evil. A couple of things can cause this. An improper prim breakdown(be sure to set prim_geom to zero after d.GeomDestroy! An improper buildup (creating the geom failed). Or, the Scene Reused a physics actor after disposing it.)");
1423 }
1424 }
1425
1426/* obsolete
1427 private void changeAngularLock(float timestep)
1428 {
1429 if (_parent == null)
1430 {
1431 m_angularLock = m_taintAngularLock;
1432 m_angularLockSet = true;
1433 }
1434 }
1435 */
1436 private void changelink(float timestep)
1437 {
1438 // If the newly set parent is not null
1439 // create link
1440 if (_parent == null && m_taintparent != null)
1441 {
1442 if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim)
1443 {
1444 OdePrim obj = (OdePrim)m_taintparent;
1445 //obj.disableBody();
1446 obj.ParentPrim(this);
1447
1448 /*
1449 if (obj.Body != (IntPtr)0 && Body != (IntPtr)0 && obj.Body != Body)
1450 {
1451 _linkJointGroup = d.JointGroupCreate(0);
1452 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
1453 d.JointAttach(m_linkJoint, obj.Body, Body);
1454 d.JointSetFixed(m_linkJoint);
1455 }
1456 */
1457 }
1458 }
1459 // If the newly set parent is null
1460 // destroy link
1461 else if (_parent != null && m_taintparent == null)
1462 {
1463 if (_parent is OdePrim)
1464 {
1465 OdePrim obj = (OdePrim)_parent;
1466 obj.ChildDelink(this);
1467 childPrim = false;
1468 //_parent = null;
1469 }
1470
1471 /*
1472 if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0)
1473 d.JointGroupDestroy(_linkJointGroup);
1474
1475 _linkJointGroup = (IntPtr)0;
1476 m_linkJoint = (IntPtr)0;
1477 */
1478 }
1479
1480 _parent = m_taintparent;
1481 m_taintPhysics = m_isphysical;
1482 }
1483
1484 // I'm the parent
1485 // prim is the child
1486 public void ParentPrim(OdePrim prim)
1487 {
1488 if (this.m_localID != prim.m_localID)
1489 {
1490 if (Body == IntPtr.Zero)
1491 {
1492 Body = d.BodyCreate(_parent_scene.world);
1493 // disconnect from world gravity so we can apply buoyancy
1494 d.BodySetGravityMode (Body, false);
1495
1496 setMass();
1497 }
1498 if (Body != IntPtr.Zero)
1499 {
1500 lock (childrenPrim)
1501 {
1502 if (!childrenPrim.Contains(prim))
1503 {
1504 childrenPrim.Add(prim);
1505
1506 foreach (OdePrim prm in childrenPrim)
1507 {
1508 d.Mass m2;
1509 d.MassSetZero(out m2);
1510 d.MassSetBoxTotal(out m2, prim.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z);
1511
1512
1513 d.Quaternion quat = new d.Quaternion();
1514 quat.W = prm._orientation.W;
1515 quat.X = prm._orientation.X;
1516 quat.Y = prm._orientation.Y;
1517 quat.Z = prm._orientation.Z;
1518
1519 d.Matrix3 mat = new d.Matrix3();
1520 d.RfromQ(out mat, ref quat);
1521 d.MassRotate(ref m2, ref mat);
1522 d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z);
1523 d.MassAdd(ref pMass, ref m2);
1524 }
1525 foreach (OdePrim prm in childrenPrim)
1526 {
1527
1528 prm.m_collisionCategories |= CollisionCategories.Body;
1529 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1530
1531 if (prm.prim_geom == IntPtr.Zero)
1532 {
1533 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet");
1534 continue;
1535 }
1536 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
1537 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
1538
1539
1540 d.Quaternion quat = new d.Quaternion();
1541 quat.W = prm._orientation.W;
1542 quat.X = prm._orientation.X;
1543 quat.Y = prm._orientation.Y;
1544 quat.Z = prm._orientation.Z;
1545
1546 d.Matrix3 mat = new d.Matrix3();
1547 d.RfromQ(out mat, ref quat);
1548 if (Body != IntPtr.Zero)
1549 {
1550 d.GeomSetBody(prm.prim_geom, Body);
1551 prm.childPrim = true;
1552 d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z);
1553 //d.GeomSetOffsetPosition(prim.prim_geom,
1554 // (Position.X - prm.Position.X) - pMass.c.X,
1555 // (Position.Y - prm.Position.Y) - pMass.c.Y,
1556 // (Position.Z - prm.Position.Z) - pMass.c.Z);
1557 d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat);
1558 //d.GeomSetOffsetRotation(prm.prim_geom, ref mat);
1559 d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
1560 d.BodySetMass(Body, ref pMass);
1561 }
1562 else
1563 {
1564 m_log.Debug("[PHYSICS]:I ain't got no boooooooooddy, no body");
1565 }
1566
1567
1568 prm.m_interpenetrationcount = 0;
1569 prm.m_collisionscore = 0;
1570 prm.m_disabled = false;
1571
1572 prm.Body = Body;
1573 _parent_scene.addActivePrim(prm);
1574 }
1575 m_collisionCategories |= CollisionCategories.Body;
1576 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1577
1578 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1579 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1580
1581
1582 d.Quaternion quat2 = new d.Quaternion();
1583 quat2.W = _orientation.W;
1584 quat2.X = _orientation.X;
1585 quat2.Y = _orientation.Y;
1586 quat2.Z = _orientation.Z;
1587
1588 d.Matrix3 mat2 = new d.Matrix3();
1589 d.RfromQ(out mat2, ref quat2);
1590 d.GeomSetBody(prim_geom, Body);
1591 d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z);
1592 //d.GeomSetOffsetPosition(prim.prim_geom,
1593 // (Position.X - prm.Position.X) - pMass.c.X,
1594 // (Position.Y - prm.Position.Y) - pMass.c.Y,
1595 // (Position.Z - prm.Position.Z) - pMass.c.Z);
1596 //d.GeomSetOffsetRotation(prim_geom, ref mat2);
1597 d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
1598 d.BodySetMass(Body, ref pMass);
1599
1600 d.BodySetAutoDisableFlag(Body, true);
1601 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
1602
1603
1604 m_interpenetrationcount = 0;
1605 m_collisionscore = 0;
1606 m_disabled = false;
1607
1608 d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
1609 if (m_type != Vehicle.TYPE_NONE) Enable(Body, _parent_scene);
1610 _parent_scene.addActivePrim(this);
1611 }
1612 }
1613 }
1614 }
1615
1616 }
1617
1618 private void ChildSetGeom(OdePrim odePrim)
1619 {
1620 //if (m_isphysical && Body != IntPtr.Zero)
1621 lock (childrenPrim)
1622 {
1623 foreach (OdePrim prm in childrenPrim)
1624 {
1625 //prm.childPrim = true;
1626 prm.disableBody();
1627 //prm.m_taintparent = null;
1628 //prm._parent = null;
1629 //prm.m_taintPhysics = false;
1630 //prm.m_disabled = true;
1631 //prm.childPrim = false;
1632 }
1633 }
1634 disableBody();
1635
1636
1637 if (Body != IntPtr.Zero)
1638 {
1639 _parent_scene.remActivePrim(this);
1640 }
1641
1642 lock (childrenPrim)
1643 {
1644 foreach (OdePrim prm in childrenPrim)
1645 {
1646 ParentPrim(prm);
1647 }
1648 }
1649
1650 }
1651
1652 private void ChildDelink(OdePrim odePrim)
1653 {
1654 // Okay, we have a delinked child.. need to rebuild the body.
1655 lock (childrenPrim)
1656 {
1657 foreach (OdePrim prm in childrenPrim)
1658 {
1659 prm.childPrim = true;
1660 prm.disableBody();
1661 //prm.m_taintparent = null;
1662 //prm._parent = null;
1663 //prm.m_taintPhysics = false;
1664 //prm.m_disabled = true;
1665 //prm.childPrim = false;
1666 }
1667 }
1668 disableBody();
1669
1670 lock (childrenPrim)
1671 {
1672 childrenPrim.Remove(odePrim);
1673 }
1674
1675 if (Body != IntPtr.Zero)
1676 {
1677 _parent_scene.remActivePrim(this);
1678 }
1679
1680 lock (childrenPrim)
1681 {
1682 foreach (OdePrim prm in childrenPrim)
1683 {
1684 ParentPrim(prm);
1685 }
1686 }
1687 }
1688
1689 private void changeSelectedStatus(float timestep)
1690 {
1691 if (m_taintselected)
1692 {
1693 m_collisionCategories = CollisionCategories.Selected;
1694 m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
1695
1696 // We do the body disable soft twice because 'in theory' a collision could have happened
1697 // in between the disabling and the collision properties setting
1698 // which would wake the physical body up from a soft disabling and potentially cause it to fall
1699 // through the ground.
1700
1701 // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select
1702 // just one part of the assembly, the rest of the assembly is non-selected and still simulating,
1703 // so that causes the selected part to wake up and continue moving.
1704
1705 // even if you select all parts of a jointed assembly, it is not guaranteed that the entire
1706 // assembly will stop simulating during the selection, because of the lack of atomicity
1707 // of select operations (their processing could be interrupted by a thread switch, causing
1708 // simulation to continue before all of the selected object notifications trickle down to
1709 // the physics engine).
1710
1711 // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are
1712 // selected and disabled. then, due to a thread switch, the selection processing is
1713 // interrupted and the physics engine continues to simulate, so the last 50 items, whose
1714 // selection was not yet processed, continues to simulate. this wakes up ALL of the
1715 // first 50 again. then the last 50 are disabled. then the first 50, which were just woken
1716 // up, start simulating again, which in turn wakes up the last 50.
1717
1718 if (m_isphysical)
1719 {
1720 disableBodySoft();
1721 }
1722
1723 if (prim_geom != IntPtr.Zero)
1724 {
1725 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1726 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1727 }
1728
1729 if (m_isphysical)
1730 {
1731 disableBodySoft();
1732 }
1733 if (Body != IntPtr.Zero)
1734 {
1735 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1736 d.BodySetForce(Body, 0f, 0f, 0f);
1737 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f);
1738 d.BodySetTorque (Body, 0.0f, 0.0f, 0.0f);
1739 }
1740
1741 }
1742 else
1743 {
1744 m_collisionCategories = CollisionCategories.Geom;
1745
1746 if (m_isphysical)
1747 m_collisionCategories |= CollisionCategories.Body;
1748
1749 m_collisionFlags = m_default_collisionFlags;
1750
1751 if (m_collidesLand)
1752 m_collisionFlags |= CollisionCategories.Land;
1753 if (m_collidesWater)
1754 m_collisionFlags |= CollisionCategories.Water;
1755
1756 if (prim_geom != IntPtr.Zero)
1757 {
1758 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1759 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1760 }
1761 if (Body != IntPtr.Zero)
1762 {
1763 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1764 d.BodySetForce(Body, 0f, 0f, 0f);
1765 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f);
1766 d.BodySetTorque (Body, 0.0f, 0.0f, 0.0f);
1767 }
1768
1769 if (m_isphysical)
1770 {
1771 if (Body != IntPtr.Zero)
1772 {
1773 enableBodySoft();
1774 }
1775 }
1776 }
1777
1778 resetCollisionAccounting();
1779 m_isSelected = m_taintselected;
1780 }//end changeSelectedStatus
1781
1782 public void ResetTaints()
1783 {
1784 m_taintposition = _position;
1785 m_taintrot = _orientation;
1786 m_taintPhysics = m_isphysical;
1787 m_taintselected = m_isSelected;
1788 m_taintsize = _size;
1789 m_taintshape = false;
1790 m_taintforce = false;
1791 m_taintdisable = false;
1792 m_taintVelocity = Vector3.Zero;
1793 }
1794
1795 public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh)
1796 {
1797 if (_mesh != null) // Special - make mesh
1798 {
1799 setMesh(_parent_scene, _mesh);
1800 }
1801 else // not a mesh
1802 {
1803 if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1) // special profile??
1804 {
1805 if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z) // Equi-size
1806 {
1807 if (((_size.X / 2f) > 0f)) // Has size
1808 {
1809 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1810 try
1811 {
1812 SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2));
1813 }
1814 catch (AccessViolationException)
1815 {
1816 m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
1817 ode.dunlock(_parent_scene.world);
1818 return;
1819 }
1820 }
1821 else
1822 {
1823 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1824 try
1825 {
1826 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1827 }
1828 catch (AccessViolationException)
1829 {
1830 m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
1831 ode.dunlock(_parent_scene.world);
1832 return;
1833 }
1834 }
1835 }
1836 else // not equi-size
1837 {
1838 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1839 try
1840 {
1841 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1842 }
1843 catch (AccessViolationException)
1844 {
1845 m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
1846 ode.dunlock(_parent_scene.world);
1847 return;
1848 }
1849 }
1850 }
1851
1852 else // not special profile
1853 {
1854 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1855 try
1856 {
1857 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1858 }
1859 catch (AccessViolationException)
1860 {
1861 m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
1862 ode.dunlock(_parent_scene.world);
1863 return;
1864 }
1865 }
1866 }
1867 }
1868
1869 public void changeadd(float timestep)
1870 {
1871 int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
1872 IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
1873
1874 if (targetspace == IntPtr.Zero)
1875 targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
1876
1877 m_targetSpace = targetspace;
1878
1879 if (_mesh == null && m_meshfailed == false)
1880 {
1881 if (_parent_scene.needsMeshing(_pbs))
1882 {
1883 // Don't need to re-enable body.. it's done in SetMesh
1884 try
1885 {
1886 _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
1887 }
1888 catch
1889 {
1890 //Don't continuously try to mesh prims when meshing has failed
1891 m_meshfailed = true;
1892 }
1893 // createmesh returns null when it's a shape that isn't a cube.
1894 // m_log.Debug(m_localID);
1895 }
1896 }
1897
1898
1899 lock (_parent_scene.OdeLock)
1900 {
1901 CreateGeom(m_targetSpace, _mesh);
1902
1903 if (prim_geom != IntPtr.Zero)
1904 {
1905 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
1906 d.Quaternion myrot = new d.Quaternion();
1907 myrot.X = _orientation.X;
1908 myrot.Y = _orientation.Y;
1909 myrot.Z = _orientation.Z;
1910 myrot.W = _orientation.W;
1911 d.GeomSetQuaternion(prim_geom, ref myrot);
1912 }
1913
1914 if (m_isphysical && Body == IntPtr.Zero)
1915 {
1916 enableBody();
1917 }
1918 }
1919
1920 changeSelectedStatus(timestep);
1921
1922 m_taintadd = false;
1923 }
1924
1925 public void changemove(float timestep)
1926 {
1927 if (m_isphysical)
1928 {
1929// if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits!
1930 if (!m_taintremove && !childPrim)
1931 {
1932 if (Body == IntPtr.Zero)
1933 enableBody();
1934 //Prim auto disable after 20 frames,
1935 //if you move it, re-enable the prim manually.
1936 if (_parent != null)
1937 {
1938 if (m_linkJoint != IntPtr.Zero)
1939 {
1940 d.JointDestroy(m_linkJoint);
1941 m_linkJoint = IntPtr.Zero;
1942 }
1943 }
1944 if (Body != IntPtr.Zero)
1945 {
1946 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
1947
1948 if (_parent != null)
1949 {
1950 OdePrim odParent = (OdePrim)_parent;
1951 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
1952 {
1953// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
1954Console.WriteLine("ODEPrim JointCreateFixed !!!");
1955 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
1956 d.JointAttach(m_linkJoint, Body, odParent.Body);
1957 d.JointSetFixed(m_linkJoint);
1958 }
1959 }
1960 d.BodyEnable(Body);
1961 if (m_type != Vehicle.TYPE_NONE)
1962 {
1963 Enable(Body, _parent_scene);
1964 }
1965 }
1966 else
1967 {
1968 m_log.Warn("[PHYSICS]: Body Still null after enableBody(). This is a crash scenario.");
1969 }
1970 }
1971 //else
1972 // {
1973 //m_log.Debug("[BUG]: race!");
1974 //}
1975 }
1976 else
1977 {
1978 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
1979 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
1980 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1981
1982 IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace);
1983 m_targetSpace = tempspace;
1984
1985 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1986 if (prim_geom != IntPtr.Zero)
1987 {
1988 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
1989
1990 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1991 d.SpaceAdd(m_targetSpace, prim_geom);
1992 }
1993 }
1994
1995 changeSelectedStatus(timestep);
1996
1997 resetCollisionAccounting();
1998 m_taintposition = _position;
1999 }
2000
2001
2002
2003 public void rotate(float timestep)
2004 {
2005 d.Quaternion myrot = new d.Quaternion();
2006 myrot.X = _orientation.X;
2007 myrot.Y = _orientation.Y;
2008 myrot.Z = _orientation.Z;
2009 myrot.W = _orientation.W;
2010 if (Body != IntPtr.Zero)
2011 {
2012 // KF: If this is a root prim do BodySet
2013 d.BodySetQuaternion(Body, ref myrot);
2014 }
2015 else
2016 {
2017 // daughter prim, do Geom set
2018 d.GeomSetQuaternion(prim_geom, ref myrot);
2019 }
2020
2021 resetCollisionAccounting();
2022 m_taintrot = _orientation;
2023 }
2024
2025 private void resetCollisionAccounting()
2026 {
2027 m_collisionscore = 0;
2028 m_interpenetrationcount = 0;
2029 m_disabled = false;
2030 }
2031
2032 public void changedisable(float timestep)
2033 {
2034 m_disabled = true;
2035 if (Body != IntPtr.Zero)
2036 {
2037 d.BodyDisable(Body);
2038 Body = IntPtr.Zero;
2039 }
2040
2041 m_taintdisable = false;
2042 }
2043
2044 public void changePhysicsStatus(float timestep)
2045 {
2046 if (m_isphysical == true)
2047 {
2048 if (Body == IntPtr.Zero)
2049 {
2050 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
2051 {
2052 changeshape(2f);
2053 }
2054 else
2055 {
2056 enableBody();
2057 }
2058 }
2059 }
2060 else
2061 {
2062 if (Body != IntPtr.Zero)
2063 {
2064 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
2065 {
2066 _mesh = null;
2067 changeadd(2f);
2068 }
2069 if (childPrim)
2070 {
2071 if (_parent != null)
2072 {
2073 OdePrim parent = (OdePrim)_parent;
2074 parent.ChildDelink(this);
2075 }
2076 }
2077 else
2078 {
2079 disableBody();
2080 }
2081 }
2082 }
2083
2084 changeSelectedStatus(timestep);
2085
2086 resetCollisionAccounting();
2087 m_taintPhysics = m_isphysical;
2088 }
2089
2090 public void changesize(float timestamp)
2091 {
2092
2093 string oldname = _parent_scene.geom_name_map[prim_geom];
2094
2095 if (_size.X <= 0) _size.X = 0.01f;
2096 if (_size.Y <= 0) _size.Y = 0.01f;
2097 if (_size.Z <= 0) _size.Z = 0.01f;
2098
2099 // Cleanup of old prim geometry
2100 if (_mesh != null)
2101 {
2102 // Cleanup meshing here
2103 }
2104 //kill body to rebuild
2105 if (IsPhysical && Body != IntPtr.Zero)
2106 {
2107 if (childPrim)
2108 {
2109 if (_parent != null)
2110 {
2111 OdePrim parent = (OdePrim)_parent;
2112 parent.ChildDelink(this);
2113 }
2114 }
2115 else
2116 {
2117 disableBody();
2118 }
2119 }
2120 if (d.SpaceQuery(m_targetSpace, prim_geom))
2121 {
2122 _parent_scene.waitForSpaceUnlock(m_targetSpace);
2123 d.SpaceRemove(m_targetSpace, prim_geom);
2124 }
2125 // we don't need to do space calculation because the client sends a position update also.
2126
2127 // Construction of new prim
2128 if (_parent_scene.needsMeshing(_pbs) && m_meshfailed == false)
2129 {
2130 float meshlod = _parent_scene.meshSculptLOD;
2131
2132 if (IsPhysical)
2133 meshlod = _parent_scene.MeshSculptphysicalLOD;
2134 // Don't need to re-enable body.. it's done in SetMesh
2135
2136 IMesh mesh = null;
2137
2138 try
2139 {
2140 if (_parent_scene.needsMeshing(_pbs))
2141 mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
2142 }
2143 catch
2144 {
2145 m_meshfailed = true;
2146 }
2147
2148 //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
2149 CreateGeom(m_targetSpace, mesh);
2150
2151
2152 }
2153 else
2154 {
2155 _mesh = null;
2156 CreateGeom(m_targetSpace, _mesh);
2157 }
2158
2159 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2160 d.Quaternion myrot = new d.Quaternion();
2161 myrot.X = _orientation.X;
2162 myrot.Y = _orientation.Y;
2163 myrot.Z = _orientation.Z;
2164 myrot.W = _orientation.W;
2165 d.GeomSetQuaternion(prim_geom, ref myrot);
2166
2167 //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
2168 if (IsPhysical && Body == IntPtr.Zero && !childPrim)
2169 {
2170 // Re creates body on size.
2171 // EnableBody also does setMass()
2172 enableBody();
2173 d.BodyEnable(Body);
2174 }
2175
2176 _parent_scene.geom_name_map[prim_geom] = oldname;
2177
2178 changeSelectedStatus(timestamp);
2179 if (childPrim)
2180 {
2181 if (_parent is OdePrim)
2182 {
2183 OdePrim parent = (OdePrim)_parent;
2184 parent.ChildSetGeom(this);
2185 }
2186 }
2187 resetCollisionAccounting();
2188 m_taintsize = _size;
2189 }
2190
2191
2192
2193 public void changefloatonwater(float timestep)
2194 {
2195 m_collidesWater = m_taintCollidesWater;
2196
2197 if (prim_geom != IntPtr.Zero)
2198 {
2199 if (m_collidesWater)
2200 {
2201 m_collisionFlags |= CollisionCategories.Water;
2202 }
2203 else
2204 {
2205 m_collisionFlags &= ~CollisionCategories.Water;
2206 }
2207 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2208 }
2209 }
2210
2211 public void changeshape(float timestamp)
2212 {
2213 string oldname = _parent_scene.geom_name_map[prim_geom];
2214
2215 // Cleanup of old prim geometry and Bodies
2216 if (IsPhysical && Body != IntPtr.Zero)
2217 {
2218 if (childPrim)
2219 {
2220 if (_parent != null)
2221 {
2222 OdePrim parent = (OdePrim)_parent;
2223 parent.ChildDelink(this);
2224 }
2225 }
2226 else
2227 {
2228 disableBody();
2229 }
2230 }
2231
2232
2233 // we don't need to do space calculation because the client sends a position update also.
2234 if (_size.X <= 0) _size.X = 0.01f;
2235 if (_size.Y <= 0) _size.Y = 0.01f;
2236 if (_size.Z <= 0) _size.Z = 0.01f;
2237 // Construction of new prim
2238
2239 if (_parent_scene.needsMeshing(_pbs) && m_meshfailed == false)
2240 {
2241 // Don't need to re-enable body.. it's done in SetMesh
2242 float meshlod = _parent_scene.meshSculptLOD;
2243
2244 if (IsPhysical)
2245 meshlod = _parent_scene.MeshSculptphysicalLOD;
2246 try
2247 {
2248 IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
2249 CreateGeom(m_targetSpace, mesh);
2250 }
2251 catch
2252 {
2253 m_meshfailed = true;
2254 }
2255 // createmesh returns null when it doesn't mesh.
2256 }
2257 else
2258 {
2259 _mesh = null;
2260 CreateGeom(m_targetSpace, null);
2261 }
2262
2263 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2264 d.Quaternion myrot = new d.Quaternion();
2265 //myrot.W = _orientation.w;
2266 myrot.W = _orientation.W;
2267 myrot.X = _orientation.X;
2268 myrot.Y = _orientation.Y;
2269 myrot.Z = _orientation.Z;
2270 d.GeomSetQuaternion(prim_geom, ref myrot);
2271
2272 //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
2273 if (IsPhysical && Body == IntPtr.Zero)
2274 {
2275 // Re creates body on size.
2276 // EnableBody also does setMass()
2277 enableBody();
2278 if (Body != IntPtr.Zero)
2279 {
2280 d.BodyEnable(Body);
2281 }
2282 }
2283 _parent_scene.geom_name_map[prim_geom] = oldname;
2284
2285 changeSelectedStatus(timestamp);
2286 if (childPrim)
2287 {
2288 if (_parent is OdePrim)
2289 {
2290 OdePrim parent = (OdePrim)_parent;
2291 parent.ChildSetGeom(this);
2292 }
2293 }
2294 resetCollisionAccounting();
2295 m_taintshape = false;
2296 }
2297
2298 public void changeAddForce(float timestamp)
2299 {
2300 if (!m_isSelected)
2301 {
2302 lock (m_forcelist)
2303 {
2304 //m_log.Info("[PHYSICS]: dequeing forcelist");
2305 if (IsPhysical)
2306 {
2307 Vector3 iforce = Vector3.Zero;
2308 int i = 0;
2309 try
2310 {
2311 for (i = 0; i < m_forcelist.Count; i++)
2312 {
2313
2314 iforce = iforce + (m_forcelist[i] * 100);
2315 }
2316 }
2317 catch (IndexOutOfRangeException)
2318 {
2319 m_forcelist = new List<Vector3>();
2320 m_collisionscore = 0;
2321 m_interpenetrationcount = 0;
2322 m_taintforce = false;
2323 return;
2324 }
2325 catch (ArgumentOutOfRangeException)
2326 {
2327 m_forcelist = new List<Vector3>();
2328 m_collisionscore = 0;
2329 m_interpenetrationcount = 0;
2330 m_taintforce = false;
2331 return;
2332 }
2333 d.BodyEnable(Body);
2334
2335 d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z);
2336 }
2337 m_forcelist.Clear();
2338 }
2339
2340 m_collisionscore = 0;
2341 m_interpenetrationcount = 0;
2342 }
2343
2344 m_taintforce = false;
2345
2346 }
2347
2348
2349
2350 public void changeSetTorque(float timestamp)
2351 {
2352 if (!m_isSelected)
2353 {
2354 if (IsPhysical && Body != IntPtr.Zero)
2355 {
2356 d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z);
2357 }
2358 }
2359
2360 m_taintTorque = Vector3.Zero;
2361 }
2362
2363 public void changeAddAngularForce(float timestamp)
2364 {
2365 if (!m_isSelected)
2366 {
2367 lock (m_angularforcelist)
2368 {
2369 //m_log.Info("[PHYSICS]: dequeing forcelist");
2370 if (IsPhysical)
2371 {
2372 Vector3 iforce = Vector3.Zero;
2373 for (int i = 0; i < m_angularforcelist.Count; i++)
2374 {
2375 iforce = iforce + (m_angularforcelist[i] * 100);
2376 }
2377 d.BodyEnable(Body);
2378 d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z);
2379
2380 }
2381 m_angularforcelist.Clear();
2382 }
2383
2384 m_collisionscore = 0;
2385 m_interpenetrationcount = 0;
2386 }
2387
2388 m_taintaddangularforce = false;
2389 }
2390
2391 private void changevelocity(float timestep)
2392 {
2393 if (!m_isSelected)
2394 {
2395 Thread.Sleep(20);
2396 if (IsPhysical)
2397 {
2398 if (Body != IntPtr.Zero)
2399 d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
2400 }
2401
2402 //resetCollisionAccounting();
2403 }
2404 m_taintVelocity = Vector3.Zero;
2405 }
2406
2407 public void UpdatePositionAndVelocity()
2408 {
2409 return; // moved to the Move () method
2410 }
2411
2412 public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
2413 {
2414 obj.I.M00 = pMat[0, 0];
2415 obj.I.M01 = pMat[0, 1];
2416 obj.I.M02 = pMat[0, 2];
2417 obj.I.M10 = pMat[1, 0];
2418 obj.I.M11 = pMat[1, 1];
2419 obj.I.M12 = pMat[1, 2];
2420 obj.I.M20 = pMat[2, 0];
2421 obj.I.M21 = pMat[2, 1];
2422 obj.I.M22 = pMat[2, 2];
2423 return obj;
2424 }
2425
2426 public override void SubscribeEvents(int ms)
2427 {
2428 m_eventsubscription = ms;
2429 _parent_scene.addCollisionEventReporting(this);
2430 }
2431
2432 public override void UnSubscribeEvents()
2433 {
2434 _parent_scene.remCollisionEventReporting(this);
2435 m_eventsubscription = 0;
2436 }
2437
2438 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
2439 {
2440 if (CollisionEventsThisFrame == null)
2441 CollisionEventsThisFrame = new CollisionEventUpdate();
2442 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
2443 }
2444
2445 public void SendCollisions()
2446 {
2447 if (CollisionEventsThisFrame == null)
2448 return;
2449
2450 base.SendCollisionUpdate(CollisionEventsThisFrame);
2451
2452 if (CollisionEventsThisFrame.m_objCollisionList.Count == 0)
2453 CollisionEventsThisFrame = null;
2454 else
2455 CollisionEventsThisFrame = new CollisionEventUpdate();
2456 }
2457
2458 public override bool SubscribedEvents()
2459 {
2460 if (m_eventsubscription > 0)
2461 return true;
2462 return false;
2463 }
2464
2465 public static Matrix4 Inverse(Matrix4 pMat)
2466 {
2467 if (determinant3x3(pMat) == 0)
2468 {
2469 return Matrix4.Identity; // should probably throw an error. singluar matrix inverse not possible
2470 }
2471
2472
2473
2474 return (Adjoint(pMat) / determinant3x3(pMat));
2475 }
2476
2477 public static Matrix4 Adjoint(Matrix4 pMat)
2478 {
2479 Matrix4 adjointMatrix = new Matrix4();
2480 for (int i=0; i<4; i++)
2481 {
2482 for (int j=0; j<4; j++)
2483 {
2484 Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j)))));
2485 }
2486 }
2487
2488 adjointMatrix = Transpose(adjointMatrix);
2489 return adjointMatrix;
2490 }
2491
2492 public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol)
2493 {
2494 Matrix4 minor = new Matrix4();
2495 int m = 0, n = 0;
2496 for (int i = 0; i < 4; i++)
2497 {
2498 if (i == iRow)
2499 continue;
2500 n = 0;
2501 for (int j = 0; j < 4; j++)
2502 {
2503 if (j == iCol)
2504 continue;
2505 Matrix4SetValue(ref minor, m,n, matrix[i, j]);
2506 n++;
2507 }
2508 m++;
2509 }
2510 return minor;
2511 }
2512
2513 public static Matrix4 Transpose(Matrix4 pMat)
2514 {
2515 Matrix4 transposeMatrix = new Matrix4();
2516 for (int i = 0; i < 4; i++)
2517 for (int j = 0; j < 4; j++)
2518 Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]);
2519 return transposeMatrix;
2520 }
2521
2522 public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val)
2523 {
2524 switch (r)
2525 {
2526 case 0:
2527 switch (c)
2528 {
2529 case 0:
2530 pMat.M11 = val;
2531 break;
2532 case 1:
2533 pMat.M12 = val;
2534 break;
2535 case 2:
2536 pMat.M13 = val;
2537 break;
2538 case 3:
2539 pMat.M14 = val;
2540 break;
2541 }
2542
2543 break;
2544 case 1:
2545 switch (c)
2546 {
2547 case 0:
2548 pMat.M21 = val;
2549 break;
2550 case 1:
2551 pMat.M22 = val;
2552 break;
2553 case 2:
2554 pMat.M23 = val;
2555 break;
2556 case 3:
2557 pMat.M24 = val;
2558 break;
2559 }
2560
2561 break;
2562 case 2:
2563 switch (c)
2564 {
2565 case 0:
2566 pMat.M31 = val;
2567 break;
2568 case 1:
2569 pMat.M32 = val;
2570 break;
2571 case 2:
2572 pMat.M33 = val;
2573 break;
2574 case 3:
2575 pMat.M34 = val;
2576 break;
2577 }
2578
2579 break;
2580 case 3:
2581 switch (c)
2582 {
2583 case 0:
2584 pMat.M41 = val;
2585 break;
2586 case 1:
2587 pMat.M42 = val;
2588 break;
2589 case 2:
2590 pMat.M43 = val;
2591 break;
2592 case 3:
2593 pMat.M44 = val;
2594 break;
2595 }
2596
2597 break;
2598 }
2599 }
2600 private static float determinant3x3(Matrix4 pMat)
2601 {
2602 float det = 0;
2603 float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2];
2604 float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0];
2605 float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1];
2606 float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2];
2607 float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0];
2608 float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1];
2609
2610 det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6);
2611 return det;
2612
2613 }
2614
2615 private static void DMassCopy(ref d.Mass src, ref d.Mass dst)
2616 {
2617 dst.c.W = src.c.W;
2618 dst.c.X = src.c.X;
2619 dst.c.Y = src.c.Y;
2620 dst.c.Z = src.c.Z;
2621 dst.mass = src.mass;
2622 dst.I.M00 = src.I.M00;
2623 dst.I.M01 = src.I.M01;
2624 dst.I.M02 = src.I.M02;
2625 dst.I.M10 = src.I.M10;
2626 dst.I.M11 = src.I.M11;
2627 dst.I.M12 = src.I.M12;
2628 dst.I.M20 = src.I.M20;
2629 dst.I.M21 = src.I.M21;
2630 dst.I.M22 = src.I.M22;
2631 }
2632
2633 public override void SetMaterial(int pMaterial)
2634 {
2635 m_material = pMaterial;
2636 }
2637
2638 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
2639 {
2640 switch (pParam)
2641 {
2642 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
2643 if (pValue < 0.01f) pValue = 0.01f;
2644 // m_angularDeflectionEfficiency = pValue;
2645 break;
2646 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
2647 if (pValue < 0.1f) pValue = 0.1f;
2648 // m_angularDeflectionTimescale = pValue;
2649 break;
2650 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
2651 if (pValue < 0.3f) pValue = 0.3f;
2652 m_angularMotorDecayTimescale = pValue;
2653 break;
2654 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
2655 if (pValue < 0.3f) pValue = 0.3f;
2656 m_angularMotorTimescale = pValue;
2657 break;
2658 case Vehicle.BANKING_EFFICIENCY:
2659 if (pValue < 0.01f) pValue = 0.01f;
2660 // m_bankingEfficiency = pValue;
2661 break;
2662 case Vehicle.BANKING_MIX:
2663 if (pValue < 0.01f) pValue = 0.01f;
2664 // m_bankingMix = pValue;
2665 break;
2666 case Vehicle.BANKING_TIMESCALE:
2667 if (pValue < 0.01f) pValue = 0.01f;
2668 // m_bankingTimescale = pValue;
2669 break;
2670 case Vehicle.BUOYANCY:
2671 if (pValue < -1f) pValue = -1f;
2672 if (pValue > 1f) pValue = 1f;
2673 m_VehicleBuoyancy = pValue;
2674 break;
2675// case Vehicle.HOVER_EFFICIENCY:
2676// if (pValue < 0f) pValue = 0f;
2677// if (pValue > 1f) pValue = 1f;
2678// m_VhoverEfficiency = pValue;
2679// break;
2680 case Vehicle.HOVER_HEIGHT:
2681 m_VhoverHeight = pValue;
2682 break;
2683 case Vehicle.HOVER_TIMESCALE:
2684 if (pValue < 0.1f) pValue = 0.1f;
2685 m_VhoverTimescale = pValue;
2686 break;
2687 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
2688 if (pValue < 0.01f) pValue = 0.01f;
2689 // m_linearDeflectionEfficiency = pValue;
2690 break;
2691 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
2692 if (pValue < 0.01f) pValue = 0.01f;
2693 // m_linearDeflectionTimescale = pValue;
2694 break;
2695 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
2696 if (pValue < 0.3f) pValue = 0.3f;
2697 m_linearMotorDecayTimescale = pValue;
2698 break;
2699 case Vehicle.LINEAR_MOTOR_TIMESCALE:
2700 if (pValue < 0.1f) pValue = 0.1f;
2701 m_linearMotorTimescale = pValue;
2702 break;
2703 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
2704 if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable
2705 if (pValue > 1.0f) pValue = 1.0f;
2706 m_verticalAttractionEfficiency = pValue;
2707 break;
2708 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
2709 if (pValue < 0.1f) pValue = 0.1f;
2710 m_verticalAttractionTimescale = pValue;
2711 break;
2712
2713 // These are vector properties but the engine lets you use a single float value to
2714 // set all of the components to the same value
2715 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
2716 if (pValue > 30f) pValue = 30f;
2717 if (pValue < 0.1f) pValue = 0.1f;
2718 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
2719 break;
2720 case Vehicle.ANGULAR_MOTOR_DIRECTION:
2721 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
2722 UpdateAngDecay();
2723 break;
2724 case Vehicle.LINEAR_FRICTION_TIMESCALE:
2725 if (pValue < 0.1f) pValue = 0.1f;
2726 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
2727 break;
2728 case Vehicle.LINEAR_MOTOR_DIRECTION:
2729 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
2730 UpdateLinDecay();
2731 break;
2732 case Vehicle.LINEAR_MOTOR_OFFSET:
2733 // m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
2734 break;
2735
2736 }
2737
2738 }//end ProcessFloatVehicleParam
2739
2740 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
2741 {
2742 switch (pParam)
2743 {
2744 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
2745 if (pValue.X > 30f) pValue.X = 30f;
2746 if (pValue.X < 0.1f) pValue.X = 0.1f;
2747 if (pValue.Y > 30f) pValue.Y = 30f;
2748 if (pValue.Y < 0.1f) pValue.Y = 0.1f;
2749 if (pValue.Z > 30f) pValue.Z = 30f;
2750 if (pValue.Z < 0.1f) pValue.Z = 0.1f;
2751 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
2752 break;
2753 case Vehicle.ANGULAR_MOTOR_DIRECTION:
2754 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
2755 // Limit requested angular speed to 2 rps= 4 pi rads/sec
2756 if(m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f;
2757 if(m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f;
2758 if(m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f;
2759 if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f;
2760 if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f;
2761 if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f;
2762 UpdateAngDecay();
2763 break;
2764 case Vehicle.LINEAR_FRICTION_TIMESCALE:
2765 if (pValue.X < 0.1f) pValue.X = 0.1f;
2766 if (pValue.Y < 0.1f) pValue.Y = 0.1f;
2767 if (pValue.Z < 0.1f) pValue.Z = 0.1f;
2768 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
2769 break;
2770 case Vehicle.LINEAR_MOTOR_DIRECTION:
2771 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); // velocity requested by LSL, for max limiting
2772 UpdateLinDecay();
2773 break;
2774 case Vehicle.LINEAR_MOTOR_OFFSET:
2775 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
2776 break;
2777 }
2778
2779 }//end ProcessVectorVehicleParam
2780
2781 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
2782 {
2783 switch (pParam)
2784 {
2785 case Vehicle.REFERENCE_FRAME:
2786 // m_referenceFrame = pValue;
2787 break;
2788 }
2789
2790 }//end ProcessRotationVehicleParam
2791
2792 internal void ProcessVehicleFlags(int pParam, bool remove)
2793 {
2794 if (remove)
2795 {
2796 m_flags &= ~((VehicleFlag)pParam);
2797 }
2798 else
2799 {
2800 m_flags |= (VehicleFlag)pParam;
2801 }
2802 }
2803
2804 internal void ProcessTypeChange(Vehicle pType)
2805 {
2806 // Set Defaults For Type
2807 m_type = pType;
2808 switch (pType)
2809 {
2810 case Vehicle.TYPE_SLED:
2811 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
2812 m_angularFrictionTimescale = new Vector3(30, 30, 30);
2813// m_lLinMotorVel = Vector3.Zero;
2814 m_linearMotorTimescale = 1000;
2815 m_linearMotorDecayTimescale = 120;
2816 m_angularMotorDirection = Vector3.Zero;
2817 m_angularMotorDVel = Vector3.Zero;
2818 m_angularMotorTimescale = 1000;
2819 m_angularMotorDecayTimescale = 120;
2820 m_VhoverHeight = 0;
2821// m_VhoverEfficiency = 1;
2822 m_VhoverTimescale = 10;
2823 m_VehicleBuoyancy = 0;
2824 // m_linearDeflectionEfficiency = 1;
2825 // m_linearDeflectionTimescale = 1;
2826 // m_angularDeflectionEfficiency = 1;
2827 // m_angularDeflectionTimescale = 1000;
2828 // m_bankingEfficiency = 0;
2829 // m_bankingMix = 1;
2830 // m_bankingTimescale = 10;
2831 // m_referenceFrame = Quaternion.Identity;
2832 m_flags &=
2833 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
2834 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
2835 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
2836 break;
2837 case Vehicle.TYPE_CAR:
2838 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
2839 m_angularFrictionTimescale = new Vector3(30, 30, 30); // was 1000, but sl max frict time is 30.
2840// m_lLinMotorVel = Vector3.Zero;
2841 m_linearMotorTimescale = 1;
2842 m_linearMotorDecayTimescale = 60;
2843 m_angularMotorDirection = Vector3.Zero;
2844 m_angularMotorDVel = Vector3.Zero;
2845 m_angularMotorTimescale = 1;
2846 m_angularMotorDecayTimescale = 0.8f;
2847 m_VhoverHeight = 0;
2848// m_VhoverEfficiency = 0;
2849 m_VhoverTimescale = 1000;
2850 m_VehicleBuoyancy = 0;
2851 // // m_linearDeflectionEfficiency = 1;
2852 // // m_linearDeflectionTimescale = 2;
2853 // // m_angularDeflectionEfficiency = 0;
2854 // m_angularDeflectionTimescale = 10;
2855 m_verticalAttractionEfficiency = 1f;
2856 m_verticalAttractionTimescale = 10f;
2857 // m_bankingEfficiency = -0.2f;
2858 // m_bankingMix = 1;
2859 // m_bankingTimescale = 1;
2860 // m_referenceFrame = Quaternion.Identity;
2861 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
2862 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY |
2863 VehicleFlag.LIMIT_MOTOR_UP);
2864 break;
2865 case Vehicle.TYPE_BOAT:
2866 m_linearFrictionTimescale = new Vector3(10, 3, 2);
2867 m_angularFrictionTimescale = new Vector3(10,10,10);
2868// m_lLinMotorVel = Vector3.Zero;
2869 m_linearMotorTimescale = 5;
2870 m_linearMotorDecayTimescale = 60;
2871 m_angularMotorDirection = Vector3.Zero;
2872 m_angularMotorDVel = Vector3.Zero;
2873 m_angularMotorTimescale = 4;
2874 m_angularMotorDecayTimescale = 4;
2875 m_VhoverHeight = 0;
2876// m_VhoverEfficiency = 0.5f;
2877 m_VhoverTimescale = 2;
2878 m_VehicleBuoyancy = 1;
2879 // m_linearDeflectionEfficiency = 0.5f;
2880 // m_linearDeflectionTimescale = 3;
2881 // m_angularDeflectionEfficiency = 0.5f;
2882 // m_angularDeflectionTimescale = 5;
2883 m_verticalAttractionEfficiency = 0.5f;
2884 m_verticalAttractionTimescale = 5f;
2885 // m_bankingEfficiency = -0.3f;
2886 // m_bankingMix = 0.8f;
2887 // m_bankingTimescale = 1;
2888 // m_referenceFrame = Quaternion.Identity;
2889 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY |
2890 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
2891 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY |
2892 VehicleFlag.LIMIT_MOTOR_UP);
2893 break;
2894 case Vehicle.TYPE_AIRPLANE:
2895 m_linearFrictionTimescale = new Vector3(200, 10, 5);
2896 m_angularFrictionTimescale = new Vector3(20, 20, 20);
2897// m_lLinMotorVel = Vector3.Zero;
2898 m_linearMotorTimescale = 2;
2899 m_linearMotorDecayTimescale = 60;
2900 m_angularMotorDirection = Vector3.Zero;
2901 m_angularMotorDVel = Vector3.Zero;
2902 m_angularMotorTimescale = 4;
2903 m_angularMotorDecayTimescale = 4;
2904 m_VhoverHeight = 0;
2905// m_VhoverEfficiency = 0.5f;
2906 m_VhoverTimescale = 1000;
2907 m_VehicleBuoyancy = 0;
2908 // m_linearDeflectionEfficiency = 0.5f;
2909 // m_linearDeflectionTimescale = 3;
2910 // m_angularDeflectionEfficiency = 1;
2911 // m_angularDeflectionTimescale = 2;
2912 m_verticalAttractionEfficiency = 0.9f;
2913 m_verticalAttractionTimescale = 2f;
2914 // m_bankingEfficiency = 1;
2915 // m_bankingMix = 0.7f;
2916 // m_bankingTimescale = 2;
2917 // m_referenceFrame = Quaternion.Identity;
2918 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
2919 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
2920 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
2921 break;
2922 case Vehicle.TYPE_BALLOON:
2923 m_linearFrictionTimescale = new Vector3(5, 5, 5);
2924 m_angularFrictionTimescale = new Vector3(10, 10, 10);
2925 m_linearMotorTimescale = 5;
2926 m_linearMotorDecayTimescale = 60;
2927 m_angularMotorDirection = Vector3.Zero;
2928 m_angularMotorDVel = Vector3.Zero;
2929 m_angularMotorTimescale = 6;
2930 m_angularMotorDecayTimescale = 10;
2931 m_VhoverHeight = 5;
2932// m_VhoverEfficiency = 0.8f;
2933 m_VhoverTimescale = 10;
2934 m_VehicleBuoyancy = 1;
2935 // m_linearDeflectionEfficiency = 0;
2936 // m_linearDeflectionTimescale = 5;
2937 // m_angularDeflectionEfficiency = 0;
2938 // m_angularDeflectionTimescale = 5;
2939 m_verticalAttractionEfficiency = 1f;
2940 m_verticalAttractionTimescale = 100f;
2941 // m_bankingEfficiency = 0;
2942 // m_bankingMix = 0.7f;
2943 // m_bankingTimescale = 5;
2944 // m_referenceFrame = Quaternion.Identity;
2945 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
2946 VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
2947 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
2948 break;
2949
2950 }
2951 }//end SetDefaultsForType
2952
2953 internal void Enable(IntPtr pBody, OdeScene pParentScene)
2954 {
2955 if (m_type == Vehicle.TYPE_NONE)
2956 return;
2957
2958 m_body = pBody;
2959 }
2960
2961
2962 internal void Halt()
2963 { // Kill all motions, when non-physical
2964 // m_linearMotorDirection = Vector3.Zero;
2965 m_lLinMotorDVel = Vector3.Zero;
2966 m_lLinObjectVel = Vector3.Zero;
2967 m_wLinObjectVel = Vector3.Zero;
2968 m_angularMotorDirection = Vector3.Zero;
2969 m_lastAngularVelocity = Vector3.Zero;
2970 m_angularMotorDVel = Vector3.Zero;
2971 _acceleration = Vector3.Zero;
2972 }
2973
2974 private void UpdateLinDecay()
2975 {
2976// if (Math.Abs(m_linearMotorDirection.X) > Math.Abs(m_lLinMotorDVel.X)) m_lLinMotorDVel.X = m_linearMotorDirection.X;
2977// if (Math.Abs(m_linearMotorDirection.Y) > Math.Abs(m_lLinMotorDVel.Y)) m_lLinMotorDVel.Y = m_linearMotorDirection.Y;
2978// if (Math.Abs(m_linearMotorDirection.Z) > Math.Abs(m_lLinMotorDVel.Z)) m_lLinMotorDVel.Z = m_linearMotorDirection.Z;
2979 m_lLinMotorDVel.X = m_linearMotorDirection.X;
2980 m_lLinMotorDVel.Y = m_linearMotorDirection.Y;
2981 m_lLinMotorDVel.Z = m_linearMotorDirection.Z;
2982 } // else let the motor decay on its own
2983
2984 private void UpdateAngDecay()
2985 {
2986// if (Math.Abs(m_angularMotorDirection.X) > Math.Abs(m_angularMotorDVel.X)) m_angularMotorDVel.X = m_angularMotorDirection.X;
2987// if (Math.Abs(m_angularMotorDirection.Y) > Math.Abs(m_angularMotorDVel.Y)) m_angularMotorDVel.Y = m_angularMotorDirection.Y;
2988// if (Math.Abs(m_angularMotorDirection.Z) > Math.Abs(m_angularMotorDVel.Z)) m_angularMotorDVel.Z = m_angularMotorDirection.Z;
2989 m_angularMotorDVel.X = m_angularMotorDirection.X;
2990 m_angularMotorDVel.Y = m_angularMotorDirection.Y;
2991 m_angularMotorDVel.Z = m_angularMotorDirection.Z;
2992 } // else let the motor decay on its own
2993
2994 public void Move(float timestep)
2995 {
2996 float fx = 0;
2997 float fy = 0;
2998 float fz = 0;
2999 Vector3 linvel; // velocity applied, including any reversal
3000 int outside = 0;
3001
3002 // If geomCrossingFailuresBeforeOutofbounds is set to 0 in OpenSim.ini then phys objects bounce off region borders.
3003 // This is a temp patch until proper region crossing is developed.
3004
3005 int failureLimit = _parent_scene.geomCrossingFailuresBeforeOutofbounds;
3006 float fence = _parent_scene.geomRegionFence;
3007
3008 frcount++; // used to limit debug comment output
3009 if (frcount > 50)
3010 frcount = 0;
3011
3012 if(revcount > 0) revcount--;
3013
3014 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // Only move root prims.
3015 {
3016 // Old public void UpdatePositionAndVelocity(), more accuratley calculated here
3017 bool lastZeroFlag = _zeroFlag; // was it stopped
3018
3019 d.Vector3 vec = d.BodyGetPosition(Body);
3020 Vector3 l_position = Vector3.Zero;
3021 l_position.X = vec.X;
3022 l_position.Y = vec.Y;
3023 l_position.Z = vec.Z;
3024 m_lastposition = _position;
3025 _position = l_position;
3026
3027 d.Quaternion ori = d.BodyGetQuaternion(Body);
3028 // Quaternion l_orientation = Quaternion.Identity;
3029 _orientation.X = ori.X;
3030 _orientation.Y = ori.Y;
3031 _orientation.Z = ori.Z;
3032 _orientation.W = ori.W;
3033 m_lastorientation = _orientation;
3034
3035 d.Vector3 vel = d.BodyGetLinearVel(Body);
3036 m_lastVelocity = _velocity;
3037 _velocity.X = vel.X;
3038 _velocity.Y = vel.Y;
3039 _velocity.Z = vel.Z;
3040 _acceleration = ((_velocity - m_lastVelocity) / timestep);
3041
3042 d.Vector3 torque = d.BodyGetTorque(Body);
3043 _torque = new Vector3(torque.X, torque.Y, torque.Z);
3044
3045 base.RequestPhysicsterseUpdate();
3046
3047//Console.WriteLine("Move {0} at {1}", m_primName, l_position);
3048
3049 // Check if outside region
3050 // In Scene.cs/CrossPrimGroupIntoNewRegion the object is checked for 0.1M from border!
3051 if (l_position.X > ((float)_parent_scene.WorldExtents.X - fence))
3052 {
3053 l_position.X = ((float)_parent_scene.WorldExtents.X - fence);
3054 outside = 1;
3055 }
3056
3057 if (l_position.X < fence)
3058 {
3059 l_position.X = fence;
3060 outside = 2;
3061 }
3062 if (l_position.Y > ((float)_parent_scene.WorldExtents.Y - fence))
3063 {
3064 l_position.Y = ((float)_parent_scene.WorldExtents.Y - fence);
3065 outside = 3;
3066 }
3067
3068 if (l_position.Y < fence)
3069 {
3070 l_position.Y = fence;
3071 outside = 4;
3072 }
3073
3074 if (outside > 0)
3075 {
3076
3077//Console.WriteLine("Border {0} fence={1}", l_position, fence);
3078 if (fence > 0.0f) // bounce object off boundary
3079 {
3080 if (revcount == 0)
3081 {
3082 if (outside < 3)
3083 {
3084 _velocity.X = -_velocity.X;
3085 }
3086 else
3087 {
3088 _velocity.Y = -_velocity.Y;
3089 }
3090 if (m_type != Vehicle.TYPE_NONE) Halt();
3091 _position = l_position;
3092 m_taintposition = _position;
3093 m_lastVelocity = _velocity;
3094 _acceleration = Vector3.Zero;
3095 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
3096 d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
3097 base.RequestPhysicsterseUpdate();
3098
3099 revcount = 25; // wait for object to move away from border
3100 }
3101 } // else old crossing mode
3102 else if (m_crossingfailures < failureLimit)
3103 { // keep trying to cross?
3104 _position = l_position;
3105 //_parent_scene.remActivePrim(this);
3106 if (_parent == null) base.RequestPhysicsterseUpdate();
3107 return; // Dont process any other motion?
3108 }
3109 else
3110 { // Too many tries
3111 if (_parent == null) base.RaiseOutOfBounds(l_position);
3112 return; // Dont process any other motion?
3113 } // end various methods
3114 } // end outside region horizontally
3115
3116
3117 if (l_position.Z < 0)
3118 {
3119 // This is so prim that get lost underground don't fall forever and suck up
3120 //
3121 // Sim resources and memory.
3122 // Disables the prim's movement physics....
3123 // It's a hack and will generate a console message if it fails.
3124
3125 //IsPhysical = false;
3126 if (_parent == null) base.RaiseOutOfBounds(_position);
3127
3128
3129 _acceleration.X = 0; // This stuff may stop client display but it has no
3130 _acceleration.Y = 0; // effect on the object in phys engine!
3131 _acceleration.Z = 0;
3132
3133 _velocity.X = 0;
3134 _velocity.Y = 0;
3135 _velocity.Z = 0;
3136 m_lastVelocity = Vector3.Zero;
3137 m_rotationalVelocity.X = 0;
3138 m_rotationalVelocity.Y = 0;
3139 m_rotationalVelocity.Z = 0;
3140
3141 if (_parent == null) base.RequestPhysicsterseUpdate();
3142
3143 m_throttleUpdates = false;
3144 throttleCounter = 0;
3145 _zeroFlag = true;
3146 //outofBounds = true;
3147 } // end neg Z check
3148
3149 // Is it moving?
3150 /* if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
3151 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
3152 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) */
3153 if ( (Vector3.Mag(_velocity) < 0.01) && // moving very slowly
3154 (Vector3.Mag(_velocity) < Vector3.Mag(m_lastVelocity)) && // decelerating
3155 (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001) ) // spinning very slowly
3156 {
3157 _zeroFlag = true;
3158 m_throttleUpdates = false;
3159 }
3160 else
3161 {
3162 //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
3163 _zeroFlag = false;
3164 m_lastUpdateSent = false;
3165 //m_throttleUpdates = false;
3166 }
3167
3168 if (_zeroFlag)
3169 { // Its stopped
3170 _velocity.X = 0.0f;
3171 _velocity.Y = 0.0f;
3172 // _velocity.Z = 0.0f;
3173
3174 _acceleration.X = 0;
3175 _acceleration.Y = 0;
3176 // _acceleration.Z = 0;
3177
3178 m_rotationalVelocity.X = 0;
3179 m_rotationalVelocity.Y = 0;
3180 m_rotationalVelocity.Z = 0;
3181 // Stop it in the phys engine
3182 d.BodySetLinearVel(Body, 0.0f, 0.0f, _velocity.Z);
3183 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f);
3184 d.BodySetForce(Body, 0f, 0f, 0f);
3185
3186 if (!m_lastUpdateSent)
3187 {
3188 m_throttleUpdates = false;
3189 throttleCounter = 0;
3190 if (_parent == null)
3191 {
3192 base.RequestPhysicsterseUpdate();
3193 }
3194
3195 m_lastUpdateSent = true;
3196 }
3197 }
3198 else
3199 { // Its moving
3200 if (lastZeroFlag != _zeroFlag)
3201 {
3202 if (_parent == null)
3203 {
3204 base.RequestPhysicsterseUpdate();
3205 }
3206 }
3207 m_lastUpdateSent = false;
3208 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
3209 {
3210 if (_parent == null)
3211 {
3212 base.RequestPhysicsterseUpdate();
3213 }
3214 }
3215 else
3216 {
3217 throttleCounter++;
3218 }
3219 }
3220 m_lastposition = l_position;
3221
3222 /// End UpdatePositionAndVelocity insert
3223
3224
3225 // Rotation lock =====================================
3226 if(m_rotateEnableUpdate)
3227 {
3228 // Snapshot current angles, set up Amotor(s)
3229 m_rotateEnableUpdate = false;
3230 m_rotateEnable = m_rotateEnableRequest;
3231//Console.WriteLine("RotEnable {0} = {1}",m_primName, m_rotateEnable);
3232
3233 if (Amotor != IntPtr.Zero)
3234 {
3235 d.JointDestroy(Amotor);
3236 Amotor = IntPtr.Zero;
3237//Console.WriteLine("Old Amotor Destroyed");
3238 }
3239
3240 if (!m_rotateEnable.ApproxEquals(Vector3.One, 0.003f))
3241 { // not all are enabled
3242 d.Quaternion r = d.BodyGetQuaternion(Body);
3243 Quaternion locrot = new Quaternion(r.X, r.Y, r.Z, r.W);
3244 // extract the axes vectors
3245 Vector3 vX = new Vector3(1f,0f,0f);
3246 Vector3 vY = new Vector3(0f,1f,0f);
3247 Vector3 vZ = new Vector3(0f,0f,1f);
3248 vX = vX * locrot;
3249 vY = vY * locrot;
3250 vZ = vZ * locrot;
3251 // snapshot the current angle vectors
3252 m_lockX = vX;
3253 m_lockY = vY;
3254 m_lockZ = vZ;
3255 // m_lockRot = locrot;
3256 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
3257 d.JointAttach(Amotor, Body, IntPtr.Zero);
3258 d.JointSetAMotorMode(Amotor, 0); // User mode??
3259//Console.WriteLine("New Amotor Created for {0}", m_primName);
3260
3261 float axisnum = 3; // how many to lock
3262 axisnum = (axisnum - (m_rotateEnable.X + m_rotateEnable.Y + m_rotateEnable.Z));
3263 d.JointSetAMotorNumAxes(Amotor,(int)axisnum);
3264//Console.WriteLine("AxisNum={0}",(int)axisnum);
3265
3266 int i = 0;
3267
3268 if (m_rotateEnable.X == 0)
3269 {
3270 d.JointSetAMotorAxis(Amotor, i, 0, m_lockX.X, m_lockX.Y, m_lockX.Z);
3271//Console.WriteLine("AxisX {0} set to {1}", i, m_lockX);
3272 i++;
3273 }
3274
3275 if (m_rotateEnable.Y == 0)
3276 {
3277 d.JointSetAMotorAxis(Amotor, i, 0, m_lockY.X, m_lockY.Y, m_lockY.Z);
3278//Console.WriteLine("AxisY {0} set to {1}", i, m_lockY);
3279 i++;
3280 }
3281
3282 if (m_rotateEnable.Z == 0)
3283 {
3284 d.JointSetAMotorAxis(Amotor, i, 0, m_lockZ.X, m_lockZ.Y, m_lockZ.Z);
3285//Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ);
3286 i++;
3287 }
3288
3289 // These lowstops and high stops are effectively (no wiggle room)
3290 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, 0f);
3291 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, 0f);
3292 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, 0f);
3293 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
3294 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
3295 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
3296 d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 0f);
3297 d.JointSetAMotorParam(Amotor, (int) dParam.Vel3, 0f);
3298 d.JointSetAMotorParam(Amotor, (int) dParam.Vel2, 0f);
3299 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f);
3300 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f);
3301 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f);
3302 } // else none are locked
3303 } // end Rotation Update
3304
3305
3306 // VEHICLE processing ==========================================
3307 if (m_type != Vehicle.TYPE_NONE)
3308 {
3309 // get body attitude
3310 d.Quaternion rot = d.BodyGetQuaternion(Body);
3311 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
3312 Quaternion irotq = Quaternion.Inverse(rotq);
3313
3314 // VEHICLE Linear Motion
3315 d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame
3316 Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z);
3317 m_lLinObjectVel = vel_now * irotq;
3318 if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate
3319 {
3320 if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f)
3321 {
3322 float decayfactor = m_linearMotorDecayTimescale/timestep;
3323 Vector3 decayAmount = (m_lLinMotorDVel/decayfactor);
3324 m_lLinMotorDVel -= decayAmount;
3325 }
3326 else
3327 {
3328 float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale)));
3329 Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep;
3330 m_lLinMotorDVel -= decel;
3331 }
3332 if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
3333 {
3334 m_lLinMotorDVel = Vector3.Zero;
3335 }
3336
3337 /* else
3338 {
3339 if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X;
3340 if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y;
3341 if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z;
3342 } */
3343 } // end linear motor decay
3344
3345 if ( (! m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
3346 {
3347 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
3348 if (m_linearMotorTimescale < 300.0f)
3349 {
3350 Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel;
3351 float linfactor = m_linearMotorTimescale/timestep;
3352 Vector3 attackAmount = (attack_error/linfactor) * 1.3f;
3353 m_lLinObjectVel += attackAmount;
3354 }
3355 if (m_linearFrictionTimescale.X < 300.0f)
3356 {
3357 float fricfactor = m_linearFrictionTimescale.X / timestep;
3358 float fricX = m_lLinObjectVel.X / fricfactor;
3359 m_lLinObjectVel.X -= fricX;
3360 }
3361 if (m_linearFrictionTimescale.Y < 300.0f)
3362 {
3363 float fricfactor = m_linearFrictionTimescale.Y / timestep;
3364 float fricY = m_lLinObjectVel.Y / fricfactor;
3365 m_lLinObjectVel.Y -= fricY;
3366 }
3367 if (m_linearFrictionTimescale.Z < 300.0f)
3368 {
3369 float fricfactor = m_linearFrictionTimescale.Z / timestep;
3370 float fricZ = m_lLinObjectVel.Z / fricfactor;
3371 m_lLinObjectVel.Z -= fricZ;
3372 }
3373 }
3374 m_wLinObjectVel = m_lLinObjectVel * rotq;
3375
3376 // Gravity and Buoyancy
3377 Vector3 grav = Vector3.Zero;
3378 if(m_VehicleBuoyancy < 1.0f)
3379 {
3380 // There is some gravity, make a gravity force vector
3381 // that is applied after object velocity.
3382 d.Mass objMass;
3383 d.BodyGetMass(Body, out objMass);
3384 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
3385 grav.Z = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force
3386 } // else its 1.0, no gravity.
3387
3388 // Hovering
3389 if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
3390 {
3391 // We should hover, get the target height
3392 d.Vector3 pos = d.BodyGetPosition(Body);
3393 if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
3394 {
3395 m_VhoverTargetHeight = _parent_scene.GetWaterLevel() + m_VhoverHeight;
3396 }
3397 else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
3398 {
3399 m_VhoverTargetHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
3400 }
3401 else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
3402 {
3403 m_VhoverTargetHeight = m_VhoverHeight;
3404 }
3405
3406 if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
3407 {
3408 // If body is aready heigher, use its height as target height
3409 if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
3410 }
3411
3412// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
3413// m_VhoverTimescale = 0f; // time to acheive height
3414// timestep is time since last frame,in secs
3415 float herr0 = pos.Z - m_VhoverTargetHeight;
3416 // Replace Vertical speed with correction figure if significant
3417 if(Math.Abs(herr0) > 0.01f )
3418 {
3419 //? d.Mass objMass;
3420 //? d.BodyGetMass(Body, out objMass);
3421 m_wLinObjectVel.Z = - ( (herr0 * timestep * 50.0f) / m_VhoverTimescale);
3422 //KF: m_VhoverEfficiency is not yet implemented
3423 }
3424 else
3425 {
3426 m_wLinObjectVel.Z = 0f;
3427 }
3428 }
3429 else
3430 { // not hovering
3431 if (m_wLinObjectVel.Z == 0f)
3432 { // Gravity rules
3433 m_wLinObjectVel.Z = vel_now.Z;
3434 } // else the motor has it
3435 }
3436 linvel = m_wLinObjectVel;
3437
3438 // Vehicle Linear Motion done =======================================
3439 // Apply velocity
3440 d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z);
3441 // apply gravity force
3442 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
3443//if(frcount == 0) Console.WriteLine("Vel={0} Force={1}",linvel , grav);
3444 // end MoveLinear()
3445
3446
3447 // MoveAngular
3448 /*
3449 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
3450
3451 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
3452 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
3453 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
3454
3455 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
3456 private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body
3457 */
3458//if(frcount == 0) Console.WriteLine("MoveAngular ");
3459
3460 d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body);
3461 Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z);
3462 angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation
3463
3464//if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel);
3465
3466 // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack.
3467 float atk_decayfactor = 23.0f / (m_angularMotorTimescale * timestep);
3468 m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor;
3469 // Decay Angular Motor 2.
3470 if (m_angularMotorDecayTimescale < 300.0f)
3471 {
3472 if ( Vector3.Mag(m_angularMotorDVel) < 1.0f)
3473 {
3474 float decayfactor = (m_angularMotorDecayTimescale)/timestep;
3475 Vector3 decayAmount = (m_angularMotorDVel/decayfactor);
3476 m_angularMotorDVel -= decayAmount;
3477 }
3478 else
3479 {
3480 Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale;
3481 m_angularMotorDVel -= decel;
3482 }
3483
3484 if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
3485 {
3486 m_angularMotorDVel = Vector3.Zero;
3487 }
3488 else
3489 {
3490 if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X;
3491 if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y;
3492 if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z;
3493 }
3494 } // end decay angular motor
3495//if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel);
3496
3497//if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel);
3498
3499 if ( (! m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
3500 { // if motor or object have motion
3501 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
3502
3503 if (m_angularMotorTimescale < 300.0f)
3504 {
3505 Vector3 attack_error = m_angularMotorDVel - angObjectVel;
3506 float angfactor = m_angularMotorTimescale/timestep;
3507 Vector3 attackAmount = (attack_error/angfactor);
3508 angObjectVel += attackAmount;
3509//if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount);
3510//if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel);
3511 }
3512
3513 angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep);
3514 angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep);
3515 angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep);
3516 } // else no signif. motion
3517
3518//if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel);
3519 // Bank section tba
3520 // Deflection section tba
3521//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel);
3522
3523
3524 /* // Rotation Axis Disables:
3525 if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f))
3526 {
3527 if (m_angularEnable.X == 0)
3528 angObjectVel.X = 0f;
3529 if (m_angularEnable.Y == 0)
3530 angObjectVel.Y = 0f;
3531 if (m_angularEnable.Z == 0)
3532 angObjectVel.Z = 0f;
3533 }
3534 */
3535 angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation
3536
3537 // Vertical attractor section
3538 Vector3 vertattr = Vector3.Zero;
3539
3540 if(m_verticalAttractionTimescale < 300)
3541 {
3542 float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep);
3543 // make a vector pointing up
3544 Vector3 verterr = Vector3.Zero;
3545 verterr.Z = 1.0f;
3546 // rotate it to Body Angle
3547 verterr = verterr * rotq;
3548 // 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.
3549 // 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
3550 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
3551
3552 if (verterr.Z < 0.0f)
3553 { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to
3554 // vertical, BUT for some reason a z-rotation is imparted to the object. TBI.
3555//Console.WriteLine("InvertFlip");
3556 verterr.X = 2.0f - verterr.X;
3557 verterr.Y = 2.0f - verterr.Y;
3558 }
3559 verterr *= 0.5f;
3560 // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt)
3561 Vector3 xyav = angObjectVel;
3562 xyav.Z = 0.0f;
3563 if ((!xyav.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f))
3564 {
3565 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
3566 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
3567 vertattr.X = verterr.Y;
3568 vertattr.Y = - verterr.X;
3569 vertattr.Z = 0f;
3570//if(frcount == 0) Console.WriteLine("VAerr=" + verterr);
3571
3572 // scaling appears better usingsquare-law
3573 float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
3574 float bounce = 1.0f - damped;
3575 // 0 = crit damp, 1 = bouncy
3576 float oavz = angObjectVel.Z; // retain z velocity
3577 // time-scaled correction, which sums, therefore is bouncy:
3578 angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce;
3579 // damped, good @ < 90:
3580 angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped);
3581 angObjectVel.Z = oavz;
3582//if(frcount == 0) Console.WriteLine("VA+");
3583//Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel);
3584 }
3585 else
3586 {
3587 // else error is very small
3588 angObjectVel.X = 0f;
3589 angObjectVel.Y = 0f;
3590//if(frcount == 0) Console.WriteLine("VA0");
3591 }
3592 } // else vertical attractor is off
3593//if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel);
3594
3595
3596 m_lastAngularVelocity = angObjectVel;
3597 // apply Angular Velocity to body
3598 d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
3599//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity);
3600
3601 } // end VEHICLES
3602 else
3603 {
3604 // Dyamics (NON-'VEHICLES') are dealt with here ================================================================
3605
3606 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
3607
3608 /// Dynamics Buoyancy
3609 //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle.
3610 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
3611 // NB Prims in ODE are no subject to global gravity
3612 // This should only affect gravity operations
3613
3614 float m_mass = CalculateMass();
3615 // calculate z-force due togravity on object.
3616 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass
3617 if ((m_usePID) && (m_PIDTau > 0.0f)) // Dynamics llMoveToTarget.
3618 {
3619 fz = 0; // llMoveToTarget ignores gravity.
3620 // it also ignores mass of object, and any physical resting on it.
3621 // Vector3 m_PIDTarget is where we are going
3622 // float m_PIDTau is time to get there
3623 fx = 0;
3624 fy = 0;
3625 d.Vector3 pos = d.BodyGetPosition(Body);
3626 Vector3 error = new Vector3(
3627 (m_PIDTarget.X - pos.X),
3628 (m_PIDTarget.Y - pos.Y),
3629 (m_PIDTarget.Z - pos.Z));
3630 if (error.ApproxEquals(Vector3.Zero,0.01f))
3631 { // Very close, Jump there and quit move
3632
3633 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3634 _target_velocity = Vector3.Zero;
3635 d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
3636 d.BodySetForce(Body, 0f, 0f, 0f);
3637 }
3638 else
3639 {
3640 float scale = 50.0f * timestep / m_PIDTau;
3641 if ((error.ApproxEquals(Vector3.Zero,0.5f)) && (_target_velocity != Vector3.Zero))
3642 {
3643 // Nearby, quit update of velocity
3644 }
3645 else
3646 { // Far, calc damped velocity
3647 _target_velocity = error * scale;
3648 }
3649 d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
3650 }
3651 } // end PID MoveToTarget
3652
3653
3654 /// Dynamics Hover ===================================================================================
3655 // Hover PID Controller can only run if the PIDcontroller is not in use.
3656 if (m_useHoverPID && !m_usePID)
3657 {
3658//Console.WriteLine("Hover " + m_primName);
3659
3660 // If we're using the PID controller, then we have no gravity
3661 fz = (-1 * _parent_scene.gravityz) * m_mass;
3662
3663 // no lock; for now it's only called from within Simulate()
3664
3665 // If the PID Controller isn't active then we set our force
3666 // calculating base velocity to the current position
3667
3668 if ((m_PIDTau < 1))
3669 {
3670 PID_G = PID_G / m_PIDTau;
3671 }
3672
3673 if ((PID_G - m_PIDTau) <= 0)
3674 {
3675 PID_G = m_PIDTau + 1;
3676 }
3677
3678
3679 // Where are we, and where are we headed?
3680 d.Vector3 pos = d.BodyGetPosition(Body);
3681// d.Vector3 vel = d.BodyGetLinearVel(Body);
3682
3683
3684 // Non-Vehicles have a limited set of Hover options.
3685 // determine what our target height really is based on HoverType
3686 switch (m_PIDHoverType)
3687 {
3688 case PIDHoverType.Ground:
3689 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3690 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3691 break;
3692 case PIDHoverType.GroundAndWater:
3693 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3694 m_waterHeight = _parent_scene.GetWaterLevel();
3695 if (m_groundHeight > m_waterHeight)
3696 {
3697 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3698 }
3699 else
3700 {
3701 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
3702 }
3703 break;
3704
3705 } // end switch (m_PIDHoverType)
3706
3707
3708 _target_velocity =
3709 new Vector3(0.0f, 0.0f,
3710 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
3711 );
3712
3713 // if velocity is zero, use position control; otherwise, velocity control
3714
3715 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
3716 {
3717 // keep track of where we stopped. No more slippin' & slidin'
3718
3719 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3720 // react to the physics scene by moving it's position.
3721 // Avatar to Avatar collisions
3722 // Prim to avatar collisions
3723 d.Vector3 dlinvel = vel;
3724 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
3725 d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z);
3726 d.BodyAddForce(Body, 0, 0, fz);
3727 //KF this prevents furthur motions return;
3728 }
3729 else
3730 {
3731 _zeroFlag = false;
3732
3733 // We're flying and colliding with something
3734 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
3735 }
3736 } // end m_useHoverPID && !m_usePID
3737
3738
3739 /// Dynamics Apply Forces ===================================================================================
3740 fx *= m_mass;
3741 fy *= m_mass;
3742 //fz *= m_mass;
3743 fx += m_force.X;
3744 fy += m_force.Y;
3745 fz += m_force.Z;
3746
3747 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
3748 if (fx != 0 || fy != 0 || fz != 0)
3749 {
3750 //m_taintdisable = true;
3751 //base.RaiseOutOfBounds(Position);
3752 //d.BodySetLinearVel(Body, fx, fy, 0f);
3753 if (!d.BodyIsEnabled(Body))
3754 {
3755 // A physical body at rest on a surface will auto-disable after a while,
3756 // this appears to re-enable it incase the surface it is upon vanishes,
3757 // and the body should fall again.
3758 d.BodySetLinearVel(Body, 0f, 0f, 0f);
3759 d.BodySetForce(Body, 0f, 0f, 0f);
3760 enableBodySoft();
3761 }
3762
3763 // 35x10 = 350n times the mass per second applied maximum.
3764 float nmax = 35f * m_mass;
3765 float nmin = -35f * m_mass;
3766
3767
3768 if (fx > nmax)
3769 fx = nmax;
3770 if (fx < nmin)
3771 fx = nmin;
3772 if (fy > nmax)
3773 fy = nmax;
3774 if (fy < nmin)
3775 fy = nmin;
3776 d.BodyAddForce(Body, fx, fy, fz);
3777 } // end apply forces
3778 } // end Vehicle/Dynamics
3779
3780 /// RotLookAt / LookAt =================================================================================
3781 if (m_useAPID)
3782 {
3783 // RotLookAt, apparently overrides all other rotation sources. Inputs:
3784 // Quaternion m_APIDTarget
3785 // float m_APIDStrength // From SL experiments, this is the time to get there
3786 // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
3787 // Also in SL the mass of the object has no effect on time to get there.
3788 // Factors:
3789 // get present body rotation
3790 float limit = 1.0f;
3791 float rscaler = 50f; // adjusts rotation damping time
3792 float lscaler = 10f; // adjusts linear damping time in llLookAt
3793 float RLAservo = 0f;
3794 Vector3 diff_axis;
3795 float diff_angle;
3796 d.Quaternion rot = d.BodyGetQuaternion(Body); // prim present rotation
3797 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
3798 Quaternion rtarget = new Quaternion();
3799
3800 if(m_APIDTarget.W == -99.9f)
3801 {
3802 // this is really a llLookAt(), x,y,z is the target vector
3803 Vector3 target = new Vector3(m_APIDTarget.X, m_APIDTarget.Y, m_APIDTarget.Z);
3804 Vector3 ospin = new Vector3(1.0f, 0.0f, 0.0f) * rotq;
3805 Vector3 error = new Vector3(0.0f, 0.0f, 0.0f);
3806 float twopi = 2.0f * (float)Math.PI;
3807 Vector3 dir = target - _position;
3808 dir.Normalize();
3809 float tzrot = (float)Math.Atan2(dir.Y, dir.X);
3810 float txy = (float)Math.Sqrt((dir.X * dir.X) + (dir.Y * dir.Y));
3811 float terot = (float)Math.Atan2(dir.Z, txy);
3812 float ozrot = (float)Math.Atan2(ospin.Y, ospin.X);
3813 float oxy = (float)Math.Sqrt((ospin.X * ospin.X) + (ospin.Y * ospin.Y));
3814 float oerot = (float)Math.Atan2(ospin.Z, oxy);
3815 float ra = 2.0f * ((rotq.W * rotq.X) + (rotq.Y * rotq.Z));
3816 float rb = 1.0f - 2.0f * ((rotq.Y * rotq.Y)+(rotq.X * rotq.X));
3817 float roll = (float)Math.Atan2(ra, rb);
3818 float errorz = tzrot - ozrot;
3819 if(errorz > (float)Math.PI) errorz -= twopi;
3820 else if(errorz < -(float)Math.PI) errorz += twopi;
3821 float errory = oerot - terot;
3822 if(errory > (float)Math.PI) errory -= twopi;
3823 else if(errory < -(float)Math.PI) errory += twopi;
3824 diff_angle = Math.Abs(errorz) + Math.Abs(errory) + Math.Abs(roll);
3825 if(diff_angle > 0.01f * m_APIDdamper)
3826 {
3827 m_APIDdamper = 1.0f;
3828 RLAservo = timestep / m_APIDStrength * rscaler;
3829 errorz *= RLAservo;
3830 errory *= RLAservo;
3831 error.X = -roll * 8.0f;
3832 error.Y = errory;
3833 error.Z = errorz;
3834 error *= rotq;
3835 d.BodySetAngularVel (Body, error.X, error.Y, error.Z);
3836 }
3837 else
3838 {
3839 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f);
3840 m_APIDdamper = 2.0f;
3841 }
3842 }
3843 else
3844 {
3845 // this is a llRotLookAt()
3846 rtarget = m_APIDTarget;
3847
3848 Quaternion rot_diff = Quaternion.Inverse(rotq) * rtarget; // difference to desired rot
3849 rot_diff.GetAxisAngle(out diff_axis, out diff_angle); // convert to axis to point at & error angle
3850//if(frcount == 0) Console.WriteLine("axis {0} angle {1}",diff_axis * 57.3f, diff_angle);
3851
3852 // diff_axis.Normalize(); it already is!
3853 if(diff_angle > 0.01f * m_APIDdamper) // diff_angle is always +ve // if there is enough error
3854 {
3855 m_APIDdamper = 1.0f;
3856 Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
3857 rotforce = rotforce * rotq;
3858 if(diff_angle > limit) diff_angle = limit; // cap the rotate rate
3859 RLAservo = timestep / m_APIDStrength * lscaler;
3860 rotforce = rotforce * RLAservo * diff_angle ;
3861 d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z);
3862//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo);
3863 }
3864 else
3865 { // close enough
3866 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f);
3867 m_APIDdamper = 2.0f;
3868 }
3869 } // end llLookAt/llRotLookAt
3870//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle);
3871 } // end m_useAPID
3872 } // end root prims
3873 } // end Move()
3874 } // end class
3875}
generated by cgit v1.1 at 2024-12-22 05:46:34 +0000