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