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