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