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