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