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