aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs')
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs3750
1 files changed, 3750 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..5e6696e
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -0,0 +1,3750 @@
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 }
808 }
809
810 public void enableBody()
811 {
812 // Don't enable this body if we're a child prim
813 // this should be taken care of in the parent function not here
814 if (!childPrim)
815 {
816 // Sets the geom to a body
817 Body = d.BodyCreate(_parent_scene.world);
818
819 setMass();
820 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
821 d.Quaternion myrot = new d.Quaternion();
822 myrot.X = _orientation.X;
823 myrot.Y = _orientation.Y;
824 myrot.Z = _orientation.Z;
825 myrot.W = _orientation.W;
826 d.BodySetQuaternion(Body, ref myrot);
827 d.GeomSetBody(prim_geom, Body);
828 m_collisionCategories |= CollisionCategories.Body;
829 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
830
831 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
832 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
833
834 d.BodySetAutoDisableFlag(Body, true);
835 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
836
837 // disconnect from world gravity so we can apply buoyancy
838 d.BodySetGravityMode (Body, false);
839
840 m_interpenetrationcount = 0;
841 m_collisionscore = 0;
842 m_disabled = false;
843
844 if (m_type != Vehicle.TYPE_NONE)
845 {
846 Enable(Body, _parent_scene);
847 }
848
849 _parent_scene.addActivePrim(this);
850 }
851 }
852
853 #region Mass Calculation
854
855 private float CalculateMass()
856 {
857 float volume = 0;
858
859 // No material is passed to the physics engines yet.. soo..
860 // we're using the m_density constant in the class definition
861
862 float returnMass = 0;
863
864 switch (_pbs.ProfileShape)
865 {
866 case ProfileShape.Square:
867 // Profile Volume
868
869 volume = _size.X*_size.Y*_size.Z;
870
871 // If the user has 'hollowed out'
872 // ProfileHollow is one of those 0 to 50000 values :P
873 // we like percentages better.. so turning into a percentage
874
875 if (((float) _pbs.ProfileHollow/50000f) > 0.0)
876 {
877 float hollowAmount = (float) _pbs.ProfileHollow/50000f;
878
879 // calculate the hollow volume by it's shape compared to the prim shape
880 float hollowVolume = 0;
881 switch (_pbs.HollowShape)
882 {
883 case HollowShape.Square:
884 case HollowShape.Same:
885 // Cube Hollow volume calculation
886 float hollowsizex = _size.X*hollowAmount;
887 float hollowsizey = _size.Y*hollowAmount;
888 float hollowsizez = _size.Z*hollowAmount;
889 hollowVolume = hollowsizex*hollowsizey*hollowsizez;
890 break;
891
892 case HollowShape.Circle:
893 // Hollow shape is a perfect cyllinder in respect to the cube's scale
894 // Cyllinder hollow volume calculation
895 float hRadius = _size.X/2;
896 float hLength = _size.Z;
897
898 // pi * r2 * h
899 hollowVolume = ((float) (Math.PI*Math.Pow(hRadius, 2)*hLength)*hollowAmount);
900 break;
901
902 case HollowShape.Triangle:
903 // Equilateral Triangular Prism volume hollow calculation
904 // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
905
906 float aLength = _size.Y;
907 // 1/2 abh
908 hollowVolume = (float) ((0.5*aLength*_size.X*_size.Z)*hollowAmount);
909 break;
910
911 default:
912 hollowVolume = 0;
913 break;
914 }
915 volume = volume - hollowVolume;
916 }
917
918 break;
919 case ProfileShape.Circle:
920 if (_pbs.PathCurve == (byte)Extrusion.Straight)
921 {
922 // Cylinder
923 float volume1 = (float)(Math.PI * Math.Pow(_size.X/2, 2) * _size.Z);
924 float volume2 = (float)(Math.PI * Math.Pow(_size.Y/2, 2) * _size.Z);
925
926 // Approximating the cylinder's irregularity.
927 if (volume1 > volume2)
928 {
929 volume = (float)volume1 - (volume1 - volume2);
930 }
931 else if (volume2 > volume1)
932 {
933 volume = (float)volume2 - (volume2 - volume1);
934 }
935 else
936 {
937 // Regular cylinder
938 volume = volume1;
939 }
940 }
941 else
942 {
943 // We don't know what the shape is yet, so use default
944 volume = _size.X * _size.Y * _size.Z;
945 }
946 // If the user has 'hollowed out'
947 // ProfileHollow is one of those 0 to 50000 values :P
948 // we like percentages better.. so turning into a percentage
949
950 if (((float)_pbs.ProfileHollow / 50000f) > 0.0)
951 {
952 float hollowAmount = (float)_pbs.ProfileHollow / 50000f;
953
954 // calculate the hollow volume by it's shape compared to the prim shape
955 float hollowVolume = 0;
956 switch (_pbs.HollowShape)
957 {
958 case HollowShape.Same:
959 case HollowShape.Circle:
960 // Hollow shape is a perfect cyllinder in respect to the cube's scale
961 // Cyllinder hollow volume calculation
962 float hRadius = _size.X / 2;
963 float hLength = _size.Z;
964
965 // pi * r2 * h
966 hollowVolume = ((float)(Math.PI * Math.Pow(hRadius, 2) * hLength) * hollowAmount);
967 break;
968
969 case HollowShape.Square:
970 // Cube Hollow volume calculation
971 float hollowsizex = _size.X * hollowAmount;
972 float hollowsizey = _size.Y * hollowAmount;
973 float hollowsizez = _size.Z * hollowAmount;
974 hollowVolume = hollowsizex * hollowsizey * hollowsizez;
975 break;
976
977 case HollowShape.Triangle:
978 // Equilateral Triangular Prism volume hollow calculation
979 // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
980
981 float aLength = _size.Y;
982 // 1/2 abh
983 hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);
984 break;
985
986 default:
987 hollowVolume = 0;
988 break;
989 }
990 volume = volume - hollowVolume;
991 }
992 break;
993
994 case ProfileShape.HalfCircle:
995 if (_pbs.PathCurve == (byte)Extrusion.Curve1)
996 {
997 if (_size.X == _size.Y && _size.Y == _size.Z)
998 {
999 // regular sphere
1000 // v = 4/3 * pi * r^3
1001 float sradius3 = (float)Math.Pow((_size.X / 2), 3);
1002 volume = (float)((4f / 3f) * Math.PI * sradius3);
1003 }
1004 else
1005 {
1006 // we treat this as a box currently
1007 volume = _size.X * _size.Y * _size.Z;
1008 }
1009 }
1010 else
1011 {
1012 // We don't know what the shape is yet, so use default
1013 volume = _size.X * _size.Y * _size.Z;
1014 }
1015 break;
1016
1017 case ProfileShape.EquilateralTriangle:
1018 /*
1019 v = (abs((xB*yA-xA*yB)+(xC*yB-xB*yC)+(xA*yC-xC*yA))/2) * h
1020
1021 // seed mesh
1022 Vertex MM = new Vertex(-0.25f, -0.45f, 0.0f);
1023 Vertex PM = new Vertex(+0.5f, 0f, 0.0f);
1024 Vertex PP = new Vertex(-0.25f, +0.45f, 0.0f);
1025 */
1026 float xA = -0.25f * _size.X;
1027 float yA = -0.45f * _size.Y;
1028
1029 float xB = 0.5f * _size.X;
1030 float yB = 0;
1031
1032 float xC = -0.25f * _size.X;
1033 float yC = 0.45f * _size.Y;
1034
1035 volume = (float)((Math.Abs((xB * yA - xA * yB) + (xC * yB - xB * yC) + (xA * yC - xC * yA)) / 2) * _size.Z);
1036
1037 // If the user has 'hollowed out'
1038 // ProfileHollow is one of those 0 to 50000 values :P
1039 // we like percentages better.. so turning into a percentage
1040 float fhollowFactor = ((float)_pbs.ProfileHollow / 1.9f);
1041 if (((float)fhollowFactor / 50000f) > 0.0)
1042 {
1043 float hollowAmount = (float)fhollowFactor / 50000f;
1044
1045 // calculate the hollow volume by it's shape compared to the prim shape
1046 float hollowVolume = 0;
1047 switch (_pbs.HollowShape)
1048 {
1049 case HollowShape.Same:
1050 case HollowShape.Triangle:
1051 // Equilateral Triangular Prism volume hollow calculation
1052 // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
1053
1054 float aLength = _size.Y;
1055 // 1/2 abh
1056 hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);
1057 break;
1058
1059 case HollowShape.Square:
1060 // Cube Hollow volume calculation
1061 float hollowsizex = _size.X * hollowAmount;
1062 float hollowsizey = _size.Y * hollowAmount;
1063 float hollowsizez = _size.Z * hollowAmount;
1064 hollowVolume = hollowsizex * hollowsizey * hollowsizez;
1065 break;
1066
1067 case HollowShape.Circle:
1068 // Hollow shape is a perfect cyllinder in respect to the cube's scale
1069 // Cyllinder hollow volume calculation
1070 float hRadius = _size.X / 2;
1071 float hLength = _size.Z;
1072
1073 // pi * r2 * h
1074 hollowVolume = ((float)((Math.PI * Math.Pow(hRadius, 2) * hLength)/2) * hollowAmount);
1075 break;
1076
1077 default:
1078 hollowVolume = 0;
1079 break;
1080 }
1081 volume = volume - hollowVolume;
1082 }
1083 break;
1084
1085 default:
1086 // we don't have all of the volume formulas yet so
1087 // use the common volume formula for all
1088 volume = _size.X*_size.Y*_size.Z;
1089 break;
1090 }
1091
1092 // Calculate Path cut effect on volume
1093 // Not exact, in the triangle hollow example
1094 // They should never be zero or less then zero..
1095 // we'll ignore it if it's less then zero
1096
1097 // ProfileEnd and ProfileBegin are values
1098 // from 0 to 50000
1099
1100 // Turning them back into percentages so that I can cut that percentage off the volume
1101
1102 float PathCutEndAmount = _pbs.ProfileEnd;
1103 float PathCutStartAmount = _pbs.ProfileBegin;
1104 if (((PathCutStartAmount + PathCutEndAmount)/50000f) > 0.0f)
1105 {
1106 float pathCutAmount = ((PathCutStartAmount + PathCutEndAmount)/50000f);
1107
1108 // Check the return amount for sanity
1109 if (pathCutAmount >= 0.99f)
1110 pathCutAmount = 0.99f;
1111
1112 volume = volume - (volume*pathCutAmount);
1113 }
1114 UInt16 taperX = _pbs.PathScaleX;
1115 UInt16 taperY = _pbs.PathScaleY;
1116 float taperFactorX = 0;
1117 float taperFactorY = 0;
1118
1119 // Mass = density * volume
1120 if (taperX != 100)
1121 {
1122 if (taperX > 100)
1123 {
1124 taperFactorX = 1.0f - ((float)taperX / 200);
1125 //m_log.Warn("taperTopFactorX: " + extr.taperTopFactorX.ToString());
1126 }
1127 else
1128 {
1129 taperFactorX = 1.0f - ((100 - (float)taperX) / 100);
1130 //m_log.Warn("taperBotFactorX: " + extr.taperBotFactorX.ToString());
1131 }
1132 volume = (float)volume * ((taperFactorX / 3f) + 0.001f);
1133 }
1134
1135 if (taperY != 100)
1136 {
1137 if (taperY > 100)
1138 {
1139 taperFactorY = 1.0f - ((float)taperY / 200);
1140 //m_log.Warn("taperTopFactorY: " + extr.taperTopFactorY.ToString());
1141 }
1142 else
1143 {
1144 taperFactorY = 1.0f - ((100 - (float)taperY) / 100);
1145 //m_log.Warn("taperBotFactorY: " + extr.taperBotFactorY.ToString());
1146 }
1147 volume = (float)volume * ((taperFactorY / 3f) + 0.001f);
1148 }
1149 returnMass = m_density*volume;
1150 if (returnMass <= 0) returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
1151
1152
1153
1154 // Recursively calculate mass
1155 bool HasChildPrim = false;
1156 lock (childrenPrim)
1157 {
1158 if (childrenPrim.Count > 0)
1159 {
1160 HasChildPrim = true;
1161 }
1162
1163 }
1164 if (HasChildPrim)
1165 {
1166 OdePrim[] childPrimArr = new OdePrim[0];
1167
1168 lock (childrenPrim)
1169 childPrimArr = childrenPrim.ToArray();
1170
1171 for (int i = 0; i < childPrimArr.Length; i++)
1172 {
1173 if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove)
1174 returnMass += childPrimArr[i].CalculateMass();
1175 // failsafe, this shouldn't happen but with OpenSim, you never know :)
1176 if (i > 256)
1177 break;
1178 }
1179 }
1180 if (returnMass > _parent_scene.maximumMassObject)
1181 returnMass = _parent_scene.maximumMassObject;
1182 return returnMass;
1183 }// end CalculateMass
1184
1185 #endregion
1186
1187 public void setMass()
1188 {
1189 if (Body != (IntPtr) 0)
1190 {
1191 float newmass = CalculateMass();
1192
1193 //m_log.Info("[PHYSICS]: New Mass: " + newmass.ToString());
1194
1195 d.MassSetBoxTotal(out pMass, newmass, _size.X, _size.Y, _size.Z);
1196 d.BodySetMass(Body, ref pMass);
1197 }
1198 }
1199
1200 public void disableBody()
1201 {
1202 //this kills the body so things like 'mesh' can re-create it.
1203 lock (this)
1204 {
1205 if (!childPrim)
1206 {
1207 if (Body != IntPtr.Zero)
1208 {
1209 _parent_scene.remActivePrim(this);
1210 m_collisionCategories &= ~CollisionCategories.Body;
1211 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
1212
1213 if (prim_geom != IntPtr.Zero)
1214 {
1215 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1216 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1217 }
1218
1219
1220 d.BodyDestroy(Body);
1221 lock (childrenPrim)
1222 {
1223 if (childrenPrim.Count > 0)
1224 {
1225 foreach (OdePrim prm in childrenPrim)
1226 {
1227 _parent_scene.remActivePrim(prm);
1228 prm.Body = IntPtr.Zero;
1229 }
1230 }
1231 }
1232 Body = IntPtr.Zero;
1233 }
1234 }
1235 else
1236 {
1237 _parent_scene.remActivePrim(this);
1238
1239 m_collisionCategories &= ~CollisionCategories.Body;
1240 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
1241
1242 if (prim_geom != IntPtr.Zero)
1243 {
1244 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1245 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1246 }
1247
1248
1249 Body = IntPtr.Zero;
1250 }
1251 }
1252 m_disabled = true;
1253 m_collisionscore = 0;
1254 }
1255
1256 private static Dictionary<IMesh, IntPtr> m_MeshToTriMeshMap = new Dictionary<IMesh, IntPtr>();
1257
1258 public void setMesh(OdeScene parent_scene, IMesh mesh)
1259 {
1260 // This sleeper is there to moderate how long it takes between
1261 // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object
1262
1263 //Thread.Sleep(10);
1264
1265 //Kill Body so that mesh can re-make the geom
1266 if (IsPhysical && Body != IntPtr.Zero)
1267 {
1268 if (childPrim)
1269 {
1270 if (_parent != null)
1271 {
1272 OdePrim parent = (OdePrim)_parent;
1273 parent.ChildDelink(this);
1274 }
1275 }
1276 else
1277 {
1278 disableBody();
1279 }
1280 }
1281
1282 IntPtr vertices, indices;
1283 int vertexCount, indexCount;
1284 int vertexStride, triStride;
1285 mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
1286 mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
1287
1288 mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
1289 if (m_MeshToTriMeshMap.ContainsKey(mesh))
1290 {
1291 _triMeshData = m_MeshToTriMeshMap[mesh];
1292 }
1293 else
1294 {
1295 _triMeshData = d.GeomTriMeshDataCreate();
1296
1297 d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
1298 d.GeomTriMeshDataPreprocess(_triMeshData);
1299 m_MeshToTriMeshMap[mesh] = _triMeshData;
1300 }
1301
1302 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1303 try
1304 {
1305 if (prim_geom == IntPtr.Zero)
1306 {
1307 SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
1308 }
1309 }
1310 catch (AccessViolationException)
1311 {
1312 m_log.Error("[PHYSICS]: MESH LOCKED");
1313 return;
1314 }
1315
1316
1317 // if (IsPhysical && Body == (IntPtr) 0)
1318 // {
1319 // Recreate the body
1320 // m_interpenetrationcount = 0;
1321 // m_collisionscore = 0;
1322
1323 // enableBody();
1324 // }
1325 }
1326
1327 public void ProcessTaints(float timestep) //=============================================================================
1328 {
1329 if (m_taintadd)
1330 {
1331 changeadd(timestep);
1332 }
1333
1334 if (prim_geom != IntPtr.Zero)
1335 {
1336 if (!_position.ApproxEquals(m_taintposition, 0f))
1337 changemove(timestep);
1338
1339 if (m_taintrot != _orientation)
1340 {
1341 if(childPrim && IsPhysical) // For physical child prim...
1342 {
1343 rotate(timestep);
1344 // KF: ODE will also rotate the parent prim!
1345 // so rotate the root back to where it was
1346 OdePrim parent = (OdePrim)_parent;
1347 parent.rotate(timestep);
1348 }
1349 else
1350 {
1351 //Just rotate the prim
1352 rotate(timestep);
1353 }
1354 }
1355 //
1356
1357 if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent))
1358 changePhysicsStatus(timestep);
1359 //
1360
1361 if (!_size.ApproxEquals(m_taintsize,0f))
1362 changesize(timestep);
1363 //
1364
1365 if (m_taintshape)
1366 changeshape(timestep);
1367 //
1368
1369 if (m_taintforce)
1370 changeAddForce(timestep);
1371
1372 if (m_taintaddangularforce)
1373 changeAddAngularForce(timestep);
1374
1375 if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f))
1376 changeSetTorque(timestep);
1377
1378 if (m_taintdisable)
1379 changedisable(timestep);
1380
1381 if (m_taintselected != m_isSelected)
1382 changeSelectedStatus(timestep);
1383
1384 if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f))
1385 changevelocity(timestep);
1386
1387 if (m_taintparent != _parent)
1388 changelink(timestep);
1389
1390 if (m_taintCollidesWater != m_collidesWater)
1391 changefloatonwater(timestep);
1392
1393 if (!m_angularEnable.ApproxEquals(m_taintAngularLock,0f))
1394 changeAngularLock(timestep);
1395
1396 }
1397 else
1398 {
1399 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.)");
1400 }
1401 }
1402
1403
1404 private void changeAngularLock(float timestep)
1405 {
1406 if (_parent == null)
1407 {
1408 m_angularEnable = m_taintAngularLock;
1409 }
1410 }
1411
1412 private void changelink(float timestep)
1413 {
1414 // If the newly set parent is not null
1415 // create link
1416 if (_parent == null && m_taintparent != null)
1417 {
1418 if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim)
1419 {
1420 OdePrim obj = (OdePrim)m_taintparent;
1421 //obj.disableBody();
1422 obj.ParentPrim(this);
1423
1424 /*
1425 if (obj.Body != (IntPtr)0 && Body != (IntPtr)0 && obj.Body != Body)
1426 {
1427 _linkJointGroup = d.JointGroupCreate(0);
1428 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
1429 d.JointAttach(m_linkJoint, obj.Body, Body);
1430 d.JointSetFixed(m_linkJoint);
1431 }
1432 */
1433 }
1434 }
1435 // If the newly set parent is null
1436 // destroy link
1437 else if (_parent != null && m_taintparent == null)
1438 {
1439 if (_parent is OdePrim)
1440 {
1441 OdePrim obj = (OdePrim)_parent;
1442 obj.ChildDelink(this);
1443 childPrim = false;
1444 //_parent = null;
1445 }
1446
1447 /*
1448 if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0)
1449 d.JointGroupDestroy(_linkJointGroup);
1450
1451 _linkJointGroup = (IntPtr)0;
1452 m_linkJoint = (IntPtr)0;
1453 */
1454 }
1455
1456 _parent = m_taintparent;
1457 m_taintPhysics = m_isphysical;
1458 }
1459
1460 // I'm the parent
1461 // prim is the child
1462 public void ParentPrim(OdePrim prim)
1463 {
1464 if (this.m_localID != prim.m_localID)
1465 {
1466 if (Body == IntPtr.Zero)
1467 {
1468 Body = d.BodyCreate(_parent_scene.world);
1469 setMass();
1470 }
1471 if (Body != IntPtr.Zero)
1472 {
1473 lock (childrenPrim)
1474 {
1475 if (!childrenPrim.Contains(prim))
1476 {
1477 childrenPrim.Add(prim);
1478
1479 foreach (OdePrim prm in childrenPrim)
1480 {
1481 d.Mass m2;
1482 d.MassSetZero(out m2);
1483 d.MassSetBoxTotal(out m2, prim.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z);
1484
1485
1486 d.Quaternion quat = new d.Quaternion();
1487 quat.W = prm._orientation.W;
1488 quat.X = prm._orientation.X;
1489 quat.Y = prm._orientation.Y;
1490 quat.Z = prm._orientation.Z;
1491
1492 d.Matrix3 mat = new d.Matrix3();
1493 d.RfromQ(out mat, ref quat);
1494 d.MassRotate(ref m2, ref mat);
1495 d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z);
1496 d.MassAdd(ref pMass, ref m2);
1497 }
1498 foreach (OdePrim prm in childrenPrim)
1499 {
1500
1501 prm.m_collisionCategories |= CollisionCategories.Body;
1502 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1503
1504 if (prm.prim_geom == IntPtr.Zero)
1505 {
1506 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet");
1507 continue;
1508 }
1509//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + m_primName);
1510 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
1511 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
1512
1513
1514 d.Quaternion quat = new d.Quaternion();
1515 quat.W = prm._orientation.W;
1516 quat.X = prm._orientation.X;
1517 quat.Y = prm._orientation.Y;
1518 quat.Z = prm._orientation.Z;
1519
1520 d.Matrix3 mat = new d.Matrix3();
1521 d.RfromQ(out mat, ref quat);
1522 if (Body != IntPtr.Zero)
1523 {
1524 d.GeomSetBody(prm.prim_geom, Body);
1525 prm.childPrim = true;
1526 d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z);
1527 //d.GeomSetOffsetPosition(prim.prim_geom,
1528 // (Position.X - prm.Position.X) - pMass.c.X,
1529 // (Position.Y - prm.Position.Y) - pMass.c.Y,
1530 // (Position.Z - prm.Position.Z) - pMass.c.Z);
1531 d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat);
1532 //d.GeomSetOffsetRotation(prm.prim_geom, ref mat);
1533 d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
1534 d.BodySetMass(Body, ref pMass);
1535 }
1536 else
1537 {
1538 m_log.Debug("[PHYSICS]:I ain't got no boooooooooddy, no body");
1539 }
1540
1541
1542 prm.m_interpenetrationcount = 0;
1543 prm.m_collisionscore = 0;
1544 prm.m_disabled = false;
1545
1546 prm.Body = Body;
1547 _parent_scene.addActivePrim(prm);
1548 }
1549 m_collisionCategories |= CollisionCategories.Body;
1550 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1551
1552//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + m_primName);
1553 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1554//Console.WriteLine(" Post GeomSetCategoryBits 2");
1555 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1556
1557
1558 d.Quaternion quat2 = new d.Quaternion();
1559 quat2.W = _orientation.W;
1560 quat2.X = _orientation.X;
1561 quat2.Y = _orientation.Y;
1562 quat2.Z = _orientation.Z;
1563
1564 d.Matrix3 mat2 = new d.Matrix3();
1565 d.RfromQ(out mat2, ref quat2);
1566 d.GeomSetBody(prim_geom, Body);
1567 d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z);
1568 //d.GeomSetOffsetPosition(prim.prim_geom,
1569 // (Position.X - prm.Position.X) - pMass.c.X,
1570 // (Position.Y - prm.Position.Y) - pMass.c.Y,
1571 // (Position.Z - prm.Position.Z) - pMass.c.Z);
1572 //d.GeomSetOffsetRotation(prim_geom, ref mat2);
1573 d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
1574 d.BodySetMass(Body, ref pMass);
1575
1576 d.BodySetAutoDisableFlag(Body, true);
1577 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
1578
1579
1580 m_interpenetrationcount = 0;
1581 m_collisionscore = 0;
1582 m_disabled = false;
1583
1584 d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
1585 if (m_type != Vehicle.TYPE_NONE) Enable(Body, _parent_scene);
1586 _parent_scene.addActivePrim(this);
1587 }
1588 }
1589 }
1590 }
1591
1592 }
1593
1594 private void ChildSetGeom(OdePrim odePrim)
1595 {
1596 //if (m_isphysical && Body != IntPtr.Zero)
1597 lock (childrenPrim)
1598 {
1599 foreach (OdePrim prm in childrenPrim)
1600 {
1601 //prm.childPrim = true;
1602 prm.disableBody();
1603 //prm.m_taintparent = null;
1604 //prm._parent = null;
1605 //prm.m_taintPhysics = false;
1606 //prm.m_disabled = true;
1607 //prm.childPrim = false;
1608 }
1609 }
1610 disableBody();
1611
1612
1613 if (Body != IntPtr.Zero)
1614 {
1615 _parent_scene.remActivePrim(this);
1616 }
1617
1618 lock (childrenPrim)
1619 {
1620 foreach (OdePrim prm in childrenPrim)
1621 {
1622 ParentPrim(prm);
1623 }
1624 }
1625
1626 }
1627
1628 private void ChildDelink(OdePrim odePrim)
1629 {
1630 // Okay, we have a delinked child.. need to rebuild the body.
1631 lock (childrenPrim)
1632 {
1633 foreach (OdePrim prm in childrenPrim)
1634 {
1635 prm.childPrim = true;
1636 prm.disableBody();
1637 //prm.m_taintparent = null;
1638 //prm._parent = null;
1639 //prm.m_taintPhysics = false;
1640 //prm.m_disabled = true;
1641 //prm.childPrim = false;
1642 }
1643 }
1644 disableBody();
1645
1646 lock (childrenPrim)
1647 {
1648 childrenPrim.Remove(odePrim);
1649 }
1650
1651 if (Body != IntPtr.Zero)
1652 {
1653 _parent_scene.remActivePrim(this);
1654 }
1655
1656 lock (childrenPrim)
1657 {
1658 foreach (OdePrim prm in childrenPrim)
1659 {
1660 ParentPrim(prm);
1661 }
1662 }
1663 }
1664
1665 private void changeSelectedStatus(float timestep)
1666 {
1667 if (m_taintselected)
1668 {
1669 m_collisionCategories = CollisionCategories.Selected;
1670 m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
1671
1672 // We do the body disable soft twice because 'in theory' a collision could have happened
1673 // in between the disabling and the collision properties setting
1674 // which would wake the physical body up from a soft disabling and potentially cause it to fall
1675 // through the ground.
1676
1677 // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select
1678 // just one part of the assembly, the rest of the assembly is non-selected and still simulating,
1679 // so that causes the selected part to wake up and continue moving.
1680
1681 // even if you select all parts of a jointed assembly, it is not guaranteed that the entire
1682 // assembly will stop simulating during the selection, because of the lack of atomicity
1683 // of select operations (their processing could be interrupted by a thread switch, causing
1684 // simulation to continue before all of the selected object notifications trickle down to
1685 // the physics engine).
1686
1687 // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are
1688 // selected and disabled. then, due to a thread switch, the selection processing is
1689 // interrupted and the physics engine continues to simulate, so the last 50 items, whose
1690 // selection was not yet processed, continues to simulate. this wakes up ALL of the
1691 // first 50 again. then the last 50 are disabled. then the first 50, which were just woken
1692 // up, start simulating again, which in turn wakes up the last 50.
1693
1694 if (m_isphysical)
1695 {
1696 disableBodySoft();
1697 }
1698
1699 if (prim_geom != IntPtr.Zero)
1700 {
1701 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1702 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1703 }
1704
1705 if (m_isphysical)
1706 {
1707 disableBodySoft();
1708 }
1709 }
1710 else
1711 {
1712 m_collisionCategories = CollisionCategories.Geom;
1713
1714 if (m_isphysical)
1715 m_collisionCategories |= CollisionCategories.Body;
1716
1717 m_collisionFlags = m_default_collisionFlags;
1718
1719 if (m_collidesLand)
1720 m_collisionFlags |= CollisionCategories.Land;
1721 if (m_collidesWater)
1722 m_collisionFlags |= CollisionCategories.Water;
1723
1724 if (prim_geom != IntPtr.Zero)
1725 {
1726 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1727 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1728 }
1729 if (m_isphysical)
1730 {
1731 if (Body != IntPtr.Zero)
1732 {
1733 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1734 d.BodySetForce(Body, 0, 0, 0);
1735 enableBodySoft();
1736 }
1737 }
1738 }
1739
1740 resetCollisionAccounting();
1741 m_isSelected = m_taintselected;
1742 }//end changeSelectedStatus
1743
1744 public void ResetTaints()
1745 {
1746 m_taintposition = _position;
1747 m_taintrot = _orientation;
1748 m_taintPhysics = m_isphysical;
1749 m_taintselected = m_isSelected;
1750 m_taintsize = _size;
1751 m_taintshape = false;
1752 m_taintforce = false;
1753 m_taintdisable = false;
1754 m_taintVelocity = Vector3.Zero;
1755 }
1756
1757 public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh)
1758 {
1759//Console.WriteLine("CreateGeom:");
1760 if (_mesh != null)
1761 {
1762 setMesh(_parent_scene, _mesh);
1763 }
1764 else
1765 {
1766 if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1)
1767 {
1768 if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z)
1769 {
1770 if (((_size.X / 2f) > 0f))
1771 {
1772 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1773 try
1774 {
1775//Console.WriteLine(" CreateGeom 1");
1776 SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2));
1777 }
1778 catch (AccessViolationException)
1779 {
1780 m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
1781 ode.dunlock(_parent_scene.world);
1782 return;
1783 }
1784 }
1785 else
1786 {
1787 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1788 try
1789 {
1790//Console.WriteLine(" CreateGeom 2");
1791 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1792 }
1793 catch (AccessViolationException)
1794 {
1795 m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
1796 ode.dunlock(_parent_scene.world);
1797 return;
1798 }
1799 }
1800 }
1801 else
1802 {
1803 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1804 try
1805 {
1806//Console.WriteLine(" CreateGeom 3");
1807 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1808 }
1809 catch (AccessViolationException)
1810 {
1811 m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
1812 ode.dunlock(_parent_scene.world);
1813 return;
1814 }
1815 }
1816 }
1817
1818 else
1819 {
1820 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1821 try
1822 {
1823//Console.WriteLine(" CreateGeom 4");
1824 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1825 }
1826 catch (AccessViolationException)
1827 {
1828 m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
1829 ode.dunlock(_parent_scene.world);
1830 return;
1831 }
1832 }
1833 }
1834 }
1835
1836 public void changeadd(float timestep)
1837 {
1838 int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
1839 IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
1840
1841 if (targetspace == IntPtr.Zero)
1842 targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
1843
1844 m_targetSpace = targetspace;
1845
1846 if (_mesh == null)
1847 {
1848 if (_parent_scene.needsMeshing(_pbs))
1849 {
1850 // Don't need to re-enable body.. it's done in SetMesh
1851 _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
1852 // createmesh returns null when it's a shape that isn't a cube.
1853 // m_log.Debug(m_localID);
1854 }
1855 }
1856
1857
1858 lock (_parent_scene.OdeLock)
1859 {
1860//Console.WriteLine("changeadd 1");
1861 CreateGeom(m_targetSpace, _mesh);
1862
1863 if (prim_geom != IntPtr.Zero)
1864 {
1865 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
1866 d.Quaternion myrot = new d.Quaternion();
1867 myrot.X = _orientation.X;
1868 myrot.Y = _orientation.Y;
1869 myrot.Z = _orientation.Z;
1870 myrot.W = _orientation.W;
1871 d.GeomSetQuaternion(prim_geom, ref myrot);
1872 }
1873
1874 if (m_isphysical && Body == IntPtr.Zero)
1875 {
1876 enableBody();
1877 }
1878 }
1879
1880 _parent_scene.geom_name_map[prim_geom] = this.m_primName;
1881 _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
1882
1883 changeSelectedStatus(timestep);
1884
1885 m_taintadd = false;
1886 }
1887
1888 public void changemove(float timestep)
1889 {
1890//Console.WriteLine("changemove for {0}", m_primName );
1891
1892 if (m_isphysical)
1893 {
1894//Console.WriteLine("phys {0} {1} {2}", m_disabled, m_taintremove, childPrim);
1895// if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits!
1896 if (!m_taintremove && !childPrim)
1897 {
1898//Console.WriteLine("physOK");
1899 if (Body == IntPtr.Zero)
1900 enableBody();
1901 //Prim auto disable after 20 frames,
1902 //if you move it, re-enable the prim manually.
1903 if (_parent != null)
1904 {
1905//Console.WriteLine("physChild");
1906 if (m_linkJoint != IntPtr.Zero)
1907 {
1908 d.JointDestroy(m_linkJoint);
1909 m_linkJoint = IntPtr.Zero;
1910 }
1911 }
1912 if (Body != IntPtr.Zero)
1913 {
1914//Console.WriteLine("physNotIPZ");
1915 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
1916
1917 if (_parent != null)
1918 {
1919 OdePrim odParent = (OdePrim)_parent;
1920 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
1921 {
1922// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
1923Console.WriteLine(" JointCreateFixed");
1924 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
1925 d.JointAttach(m_linkJoint, Body, odParent.Body);
1926 d.JointSetFixed(m_linkJoint);
1927 }
1928 }
1929 d.BodyEnable(Body);
1930 if (m_type != Vehicle.TYPE_NONE)
1931 {
1932 Enable(Body, _parent_scene);
1933 }
1934 }
1935 else
1936 {
1937 m_log.Warn("[PHYSICS]: Body Still null after enableBody(). This is a crash scenario.");
1938 }
1939 }
1940 //else
1941 // {
1942 //m_log.Debug("[BUG]: race!");
1943 //}
1944 }
1945 else
1946 {
1947//Console.WriteLine("NONphys");
1948 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
1949 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
1950 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1951
1952 IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace);
1953 m_targetSpace = tempspace;
1954
1955 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1956 if (prim_geom != IntPtr.Zero)
1957 {
1958 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
1959
1960 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1961 d.SpaceAdd(m_targetSpace, prim_geom);
1962 }
1963 }
1964
1965 changeSelectedStatus(timestep);
1966
1967 resetCollisionAccounting();
1968 m_taintposition = _position;
1969 }
1970
1971
1972
1973 public void rotate(float timestep)
1974 {
1975 d.Quaternion myrot = new d.Quaternion();
1976 myrot.X = _orientation.X;
1977 myrot.Y = _orientation.Y;
1978 myrot.Z = _orientation.Z;
1979 myrot.W = _orientation.W;
1980 if (Body != IntPtr.Zero)
1981 {
1982 // KF: If this is a root prim do BodySet
1983 d.BodySetQuaternion(Body, ref myrot);
1984 }
1985 else
1986 {
1987 // daughter prim, do Geom set
1988 d.GeomSetQuaternion(prim_geom, ref myrot);
1989 }
1990
1991 resetCollisionAccounting();
1992 m_taintrot = _orientation;
1993 }
1994
1995 private void resetCollisionAccounting()
1996 {
1997 m_collisionscore = 0;
1998 m_interpenetrationcount = 0;
1999 m_disabled = false;
2000 }
2001
2002 public void changedisable(float timestep)
2003 {
2004 m_disabled = true;
2005 if (Body != IntPtr.Zero)
2006 {
2007 d.BodyDisable(Body);
2008 Body = IntPtr.Zero;
2009 }
2010
2011 m_taintdisable = false;
2012 }
2013
2014 public void changePhysicsStatus(float timestep)
2015 {
2016 if (m_isphysical == true)
2017 {
2018 if (Body == IntPtr.Zero)
2019 {
2020 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
2021 {
2022 changeshape(2f);
2023 }
2024 else
2025 {
2026 enableBody();
2027 }
2028 }
2029 }
2030 else
2031 {
2032 if (Body != IntPtr.Zero)
2033 {
2034 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
2035 {
2036
2037
2038 if (prim_geom != IntPtr.Zero)
2039 {
2040 try
2041 {
2042 d.GeomDestroy(prim_geom);
2043 prim_geom = IntPtr.Zero;
2044 _mesh = null;
2045 }
2046 catch (System.AccessViolationException)
2047 {
2048 prim_geom = IntPtr.Zero;
2049 m_log.Error("[PHYSICS]: PrimGeom dead");
2050 }
2051 }
2052//Console.WriteLine("changePhysicsStatus for " + m_primName );
2053 changeadd(2f);
2054 }
2055 if (childPrim)
2056 {
2057 if (_parent != null)
2058 {
2059 OdePrim parent = (OdePrim)_parent;
2060 parent.ChildDelink(this);
2061 }
2062 }
2063 else
2064 {
2065 disableBody();
2066 }
2067 }
2068 }
2069
2070 changeSelectedStatus(timestep);
2071
2072 resetCollisionAccounting();
2073 m_taintPhysics = m_isphysical;
2074 }
2075
2076 public void changesize(float timestamp)
2077 {
2078
2079 string oldname = _parent_scene.geom_name_map[prim_geom];
2080
2081 if (_size.X <= 0) _size.X = 0.01f;
2082 if (_size.Y <= 0) _size.Y = 0.01f;
2083 if (_size.Z <= 0) _size.Z = 0.01f;
2084
2085 // Cleanup of old prim geometry
2086 if (_mesh != null)
2087 {
2088 // Cleanup meshing here
2089 }
2090 //kill body to rebuild
2091 if (IsPhysical && Body != IntPtr.Zero)
2092 {
2093 if (childPrim)
2094 {
2095 if (_parent != null)
2096 {
2097 OdePrim parent = (OdePrim)_parent;
2098 parent.ChildDelink(this);
2099 }
2100 }
2101 else
2102 {
2103 disableBody();
2104 }
2105 }
2106 if (d.SpaceQuery(m_targetSpace, prim_geom))
2107 {
2108 _parent_scene.waitForSpaceUnlock(m_targetSpace);
2109 d.SpaceRemove(m_targetSpace, prim_geom);
2110 }
2111 d.GeomDestroy(prim_geom);
2112 prim_geom = IntPtr.Zero;
2113 // we don't need to do space calculation because the client sends a position update also.
2114
2115 // Construction of new prim
2116 if (_parent_scene.needsMeshing(_pbs))
2117 {
2118 float meshlod = _parent_scene.meshSculptLOD;
2119
2120 if (IsPhysical)
2121 meshlod = _parent_scene.MeshSculptphysicalLOD;
2122 // Don't need to re-enable body.. it's done in SetMesh
2123
2124 IMesh mesh = null;
2125
2126 if (_parent_scene.needsMeshing(_pbs))
2127 mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
2128
2129 //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
2130//Console.WriteLine("changesize 1");
2131 CreateGeom(m_targetSpace, mesh);
2132
2133
2134 }
2135 else
2136 {
2137 _mesh = null;
2138//Console.WriteLine("changesize 2");
2139 CreateGeom(m_targetSpace, _mesh);
2140 }
2141
2142 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2143 d.Quaternion myrot = new d.Quaternion();
2144 myrot.X = _orientation.X;
2145 myrot.Y = _orientation.Y;
2146 myrot.Z = _orientation.Z;
2147 myrot.W = _orientation.W;
2148 d.GeomSetQuaternion(prim_geom, ref myrot);
2149
2150 //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
2151 if (IsPhysical && Body == IntPtr.Zero && !childPrim)
2152 {
2153 // Re creates body on size.
2154 // EnableBody also does setMass()
2155 enableBody();
2156 d.BodyEnable(Body);
2157 }
2158
2159 _parent_scene.geom_name_map[prim_geom] = oldname;
2160
2161 changeSelectedStatus(timestamp);
2162 if (childPrim)
2163 {
2164 if (_parent is OdePrim)
2165 {
2166 OdePrim parent = (OdePrim)_parent;
2167 parent.ChildSetGeom(this);
2168 }
2169 }
2170 resetCollisionAccounting();
2171 m_taintsize = _size;
2172 }
2173
2174
2175
2176 public void changefloatonwater(float timestep)
2177 {
2178 m_collidesWater = m_taintCollidesWater;
2179
2180 if (prim_geom != IntPtr.Zero)
2181 {
2182 if (m_collidesWater)
2183 {
2184 m_collisionFlags |= CollisionCategories.Water;
2185 }
2186 else
2187 {
2188 m_collisionFlags &= ~CollisionCategories.Water;
2189 }
2190 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2191 }
2192 }
2193
2194 public void changeshape(float timestamp)
2195 {
2196 string oldname = _parent_scene.geom_name_map[prim_geom];
2197
2198 // Cleanup of old prim geometry and Bodies
2199 if (IsPhysical && Body != IntPtr.Zero)
2200 {
2201 if (childPrim)
2202 {
2203 if (_parent != null)
2204 {
2205 OdePrim parent = (OdePrim)_parent;
2206 parent.ChildDelink(this);
2207 }
2208 }
2209 else
2210 {
2211 disableBody();
2212 }
2213 }
2214 try
2215 {
2216 d.GeomDestroy(prim_geom);
2217 }
2218 catch (System.AccessViolationException)
2219 {
2220 prim_geom = IntPtr.Zero;
2221 m_log.Error("[PHYSICS]: PrimGeom dead");
2222 }
2223 prim_geom = IntPtr.Zero;
2224 // we don't need to do space calculation because the client sends a position update also.
2225 if (_size.X <= 0) _size.X = 0.01f;
2226 if (_size.Y <= 0) _size.Y = 0.01f;
2227 if (_size.Z <= 0) _size.Z = 0.01f;
2228 // Construction of new prim
2229
2230 if (_parent_scene.needsMeshing(_pbs))
2231 {
2232 // Don't need to re-enable body.. it's done in SetMesh
2233 float meshlod = _parent_scene.meshSculptLOD;
2234
2235 if (IsPhysical)
2236 meshlod = _parent_scene.MeshSculptphysicalLOD;
2237
2238 IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
2239 // createmesh returns null when it doesn't mesh.
2240 CreateGeom(m_targetSpace, mesh);
2241 }
2242 else
2243 {
2244 _mesh = null;
2245//Console.WriteLine("changeshape");
2246 CreateGeom(m_targetSpace, null);
2247 }
2248
2249 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2250 d.Quaternion myrot = new d.Quaternion();
2251 //myrot.W = _orientation.w;
2252 myrot.W = _orientation.W;
2253 myrot.X = _orientation.X;
2254 myrot.Y = _orientation.Y;
2255 myrot.Z = _orientation.Z;
2256 d.GeomSetQuaternion(prim_geom, ref myrot);
2257
2258 //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
2259 if (IsPhysical && Body == IntPtr.Zero)
2260 {
2261 // Re creates body on size.
2262 // EnableBody also does setMass()
2263 enableBody();
2264 if (Body != IntPtr.Zero)
2265 {
2266 d.BodyEnable(Body);
2267 }
2268 }
2269 _parent_scene.geom_name_map[prim_geom] = oldname;
2270
2271 changeSelectedStatus(timestamp);
2272 if (childPrim)
2273 {
2274 if (_parent is OdePrim)
2275 {
2276 OdePrim parent = (OdePrim)_parent;
2277 parent.ChildSetGeom(this);
2278 }
2279 }
2280 resetCollisionAccounting();
2281 m_taintshape = false;
2282 }
2283
2284 public void changeAddForce(float timestamp)
2285 {
2286 if (!m_isSelected)
2287 {
2288 lock (m_forcelist)
2289 {
2290 //m_log.Info("[PHYSICS]: dequeing forcelist");
2291 if (IsPhysical)
2292 {
2293 Vector3 iforce = Vector3.Zero;
2294 int i = 0;
2295 try
2296 {
2297 for (i = 0; i < m_forcelist.Count; i++)
2298 {
2299
2300 iforce = iforce + (m_forcelist[i] * 100);
2301 }
2302 }
2303 catch (IndexOutOfRangeException)
2304 {
2305 m_forcelist = new List<Vector3>();
2306 m_collisionscore = 0;
2307 m_interpenetrationcount = 0;
2308 m_taintforce = false;
2309 return;
2310 }
2311 catch (ArgumentOutOfRangeException)
2312 {
2313 m_forcelist = new List<Vector3>();
2314 m_collisionscore = 0;
2315 m_interpenetrationcount = 0;
2316 m_taintforce = false;
2317 return;
2318 }
2319 d.BodyEnable(Body);
2320 d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z);
2321 }
2322 m_forcelist.Clear();
2323 }
2324
2325 m_collisionscore = 0;
2326 m_interpenetrationcount = 0;
2327 }
2328
2329 m_taintforce = false;
2330
2331 }
2332
2333
2334
2335 public void changeSetTorque(float timestamp)
2336 {
2337 if (!m_isSelected)
2338 {
2339 if (IsPhysical && Body != IntPtr.Zero)
2340 {
2341 d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z);
2342 }
2343 }
2344
2345 m_taintTorque = Vector3.Zero;
2346 }
2347
2348 public void changeAddAngularForce(float timestamp)
2349 {
2350 if (!m_isSelected)
2351 {
2352 lock (m_angularforcelist)
2353 {
2354 //m_log.Info("[PHYSICS]: dequeing forcelist");
2355 if (IsPhysical)
2356 {
2357 Vector3 iforce = Vector3.Zero;
2358 for (int i = 0; i < m_angularforcelist.Count; i++)
2359 {
2360 iforce = iforce + (m_angularforcelist[i] * 100);
2361 }
2362 d.BodyEnable(Body);
2363 d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z);
2364
2365 }
2366 m_angularforcelist.Clear();
2367 }
2368
2369 m_collisionscore = 0;
2370 m_interpenetrationcount = 0;
2371 }
2372
2373 m_taintaddangularforce = false;
2374 }
2375
2376 private void changevelocity(float timestep)
2377 {
2378 if (!m_isSelected)
2379 {
2380 Thread.Sleep(20);
2381 if (IsPhysical)
2382 {
2383 if (Body != IntPtr.Zero)
2384 {
2385 d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
2386 }
2387 }
2388
2389 //resetCollisionAccounting();
2390 }
2391 m_taintVelocity = Vector3.Zero;
2392 }
2393
2394 public void UpdatePositionAndVelocity()
2395 {
2396 return; // moved to the Move() method
2397 }
2398/* No one uses this?
2399 public Matrix4 FromDMass(d.Mass pMass)
2400 {
2401 Matrix4 obj;
2402 obj.M11 = pMass.I.M00;
2403 obj.M12 = pMass.I.M01;
2404 obj.M13 = pMass.I.M02;
2405 obj.M14 = 0;
2406 obj.M21 = pMass.I.M10;
2407 obj.M22 = pMass.I.M11;
2408 obj.M23 = pMass.I.M12;
2409 obj.M24 = 0;
2410 obj.M31 = pMass.I.M20;
2411 obj.M32 = pMass.I.M21;
2412 obj.M33 = pMass.I.M22;
2413 obj.M34 = 0;
2414 obj.M41 = 0;
2415 obj.M42 = 0;
2416 obj.M43 = 0;
2417 obj.M44 = 1;
2418 return obj;
2419 }
2420*/
2421 public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
2422 {
2423 obj.I.M00 = pMat[0, 0];
2424 obj.I.M01 = pMat[0, 1];
2425 obj.I.M02 = pMat[0, 2];
2426 obj.I.M10 = pMat[1, 0];
2427 obj.I.M11 = pMat[1, 1];
2428 obj.I.M12 = pMat[1, 2];
2429 obj.I.M20 = pMat[2, 0];
2430 obj.I.M21 = pMat[2, 1];
2431 obj.I.M22 = pMat[2, 2];
2432 return obj;
2433 }
2434
2435 public override void SubscribeEvents(int ms)
2436 {
2437 m_eventsubscription = ms;
2438 _parent_scene.addCollisionEventReporting(this);
2439 }
2440
2441 public override void UnSubscribeEvents()
2442 {
2443 _parent_scene.remCollisionEventReporting(this);
2444 m_eventsubscription = 0;
2445 }
2446
2447 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
2448 {
2449 if (CollisionEventsThisFrame == null)
2450 CollisionEventsThisFrame = new CollisionEventUpdate();
2451 CollisionEventsThisFrame.addCollider(CollidedWith, contact);
2452 }
2453
2454 public void SendCollisions()
2455 {
2456 if (CollisionEventsThisFrame == null)
2457 return;
2458
2459 base.SendCollisionUpdate(CollisionEventsThisFrame);
2460
2461 if (CollisionEventsThisFrame.m_objCollisionList.Count == 0)
2462 CollisionEventsThisFrame = null;
2463 else
2464 CollisionEventsThisFrame = new CollisionEventUpdate();
2465 }
2466
2467 public override bool SubscribedEvents()
2468 {
2469 if (m_eventsubscription > 0)
2470 return true;
2471 return false;
2472 }
2473
2474 public static Matrix4 Inverse(Matrix4 pMat)
2475 {
2476 if (determinant3x3(pMat) == 0)
2477 {
2478 return Matrix4.Identity; // should probably throw an error. singluar matrix inverse not possible
2479 }
2480
2481
2482
2483 return (Adjoint(pMat) / determinant3x3(pMat));
2484 }
2485
2486 public static Matrix4 Adjoint(Matrix4 pMat)
2487 {
2488 Matrix4 adjointMatrix = new Matrix4();
2489 for (int i=0; i<4; i++)
2490 {
2491 for (int j=0; j<4; j++)
2492 {
2493 Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j)))));
2494 }
2495 }
2496
2497 adjointMatrix = Transpose(adjointMatrix);
2498 return adjointMatrix;
2499 }
2500
2501 public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol)
2502 {
2503 Matrix4 minor = new Matrix4();
2504 int m = 0, n = 0;
2505 for (int i = 0; i < 4; i++)
2506 {
2507 if (i == iRow)
2508 continue;
2509 n = 0;
2510 for (int j = 0; j < 4; j++)
2511 {
2512 if (j == iCol)
2513 continue;
2514 Matrix4SetValue(ref minor, m,n, matrix[i, j]);
2515 n++;
2516 }
2517 m++;
2518 }
2519 return minor;
2520 }
2521
2522 public static Matrix4 Transpose(Matrix4 pMat)
2523 {
2524 Matrix4 transposeMatrix = new Matrix4();
2525 for (int i = 0; i < 4; i++)
2526 for (int j = 0; j < 4; j++)
2527 Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]);
2528 return transposeMatrix;
2529 }
2530
2531 public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val)
2532 {
2533 switch (r)
2534 {
2535 case 0:
2536 switch (c)
2537 {
2538 case 0:
2539 pMat.M11 = val;
2540 break;
2541 case 1:
2542 pMat.M12 = val;
2543 break;
2544 case 2:
2545 pMat.M13 = val;
2546 break;
2547 case 3:
2548 pMat.M14 = val;
2549 break;
2550 }
2551
2552 break;
2553 case 1:
2554 switch (c)
2555 {
2556 case 0:
2557 pMat.M21 = val;
2558 break;
2559 case 1:
2560 pMat.M22 = val;
2561 break;
2562 case 2:
2563 pMat.M23 = val;
2564 break;
2565 case 3:
2566 pMat.M24 = val;
2567 break;
2568 }
2569
2570 break;
2571 case 2:
2572 switch (c)
2573 {
2574 case 0:
2575 pMat.M31 = val;
2576 break;
2577 case 1:
2578 pMat.M32 = val;
2579 break;
2580 case 2:
2581 pMat.M33 = val;
2582 break;
2583 case 3:
2584 pMat.M34 = val;
2585 break;
2586 }
2587
2588 break;
2589 case 3:
2590 switch (c)
2591 {
2592 case 0:
2593 pMat.M41 = val;
2594 break;
2595 case 1:
2596 pMat.M42 = val;
2597 break;
2598 case 2:
2599 pMat.M43 = val;
2600 break;
2601 case 3:
2602 pMat.M44 = val;
2603 break;
2604 }
2605
2606 break;
2607 }
2608 }
2609 private static float determinant3x3(Matrix4 pMat)
2610 {
2611 float det = 0;
2612 float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2];
2613 float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0];
2614 float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1];
2615 float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2];
2616 float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0];
2617 float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1];
2618
2619 det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6);
2620 return det;
2621
2622 }
2623
2624 private static void DMassCopy(ref d.Mass src, ref d.Mass dst)
2625 {
2626 dst.c.W = src.c.W;
2627 dst.c.X = src.c.X;
2628 dst.c.Y = src.c.Y;
2629 dst.c.Z = src.c.Z;
2630 dst.mass = src.mass;
2631 dst.I.M00 = src.I.M00;
2632 dst.I.M01 = src.I.M01;
2633 dst.I.M02 = src.I.M02;
2634 dst.I.M10 = src.I.M10;
2635 dst.I.M11 = src.I.M11;
2636 dst.I.M12 = src.I.M12;
2637 dst.I.M20 = src.I.M20;
2638 dst.I.M21 = src.I.M21;
2639 dst.I.M22 = src.I.M22;
2640 }
2641
2642 public override void SetMaterial(int pMaterial)
2643 {
2644 m_material = pMaterial;
2645 }
2646
2647 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
2648 {
2649 switch (pParam)
2650 {
2651 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
2652 if (pValue < 0.01f) pValue = 0.01f;
2653 // m_angularDeflectionEfficiency = pValue;
2654 break;
2655 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
2656 if (pValue < 0.01f) pValue = 0.01f;
2657 // m_angularDeflectionTimescale = pValue;
2658 break;
2659 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
2660 if (pValue < 0.01f) pValue = 0.01f;
2661 m_angularMotorDecayTimescale = pValue;
2662 break;
2663 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
2664 if (pValue < 0.01f) pValue = 0.01f;
2665 m_angularMotorTimescale = pValue;
2666 break;
2667 case Vehicle.BANKING_EFFICIENCY:
2668 if (pValue < 0.01f) pValue = 0.01f;
2669 // m_bankingEfficiency = pValue;
2670 break;
2671 case Vehicle.BANKING_MIX:
2672 if (pValue < 0.01f) pValue = 0.01f;
2673 // m_bankingMix = pValue;
2674 break;
2675 case Vehicle.BANKING_TIMESCALE:
2676 if (pValue < 0.01f) pValue = 0.01f;
2677 // m_bankingTimescale = pValue;
2678 break;
2679 case Vehicle.BUOYANCY:
2680 if (pValue < -1f) pValue = -1f;
2681 if (pValue > 1f) pValue = 1f;
2682 m_VehicleBuoyancy = pValue;
2683 break;
2684// case Vehicle.HOVER_EFFICIENCY:
2685// if (pValue < 0f) pValue = 0f;
2686// if (pValue > 1f) pValue = 1f;
2687// m_VhoverEfficiency = pValue;
2688// break;
2689 case Vehicle.HOVER_HEIGHT:
2690 m_VhoverHeight = pValue;
2691 break;
2692 case Vehicle.HOVER_TIMESCALE:
2693 if (pValue < 0.01f) pValue = 0.01f;
2694 m_VhoverTimescale = pValue;
2695 break;
2696 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
2697 if (pValue < 0.01f) pValue = 0.01f;
2698 // m_linearDeflectionEfficiency = pValue;
2699 break;
2700 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
2701 if (pValue < 0.01f) pValue = 0.01f;
2702 // m_linearDeflectionTimescale = pValue;
2703 break;
2704 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
2705 if (pValue < 0.01f) pValue = 0.01f;
2706 m_linearMotorDecayTimescale = pValue;
2707 break;
2708 case Vehicle.LINEAR_MOTOR_TIMESCALE:
2709 if (pValue < 0.01f) pValue = 0.01f;
2710 m_linearMotorTimescale = pValue;
2711 break;
2712 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
2713 if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable
2714 if (pValue > 1.0f) pValue = 1.0f;
2715 m_verticalAttractionEfficiency = pValue;
2716 break;
2717 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
2718 if (pValue < 0.01f) pValue = 0.01f;
2719 m_verticalAttractionTimescale = pValue;
2720 break;
2721
2722 // These are vector properties but the engine lets you use a single float value to
2723 // set all of the components to the same value
2724 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
2725 if (pValue > 30f) pValue = 30f;
2726 if (pValue < 0.1f) pValue = 0.1f;
2727 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
2728 break;
2729 case Vehicle.ANGULAR_MOTOR_DIRECTION:
2730 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
2731 UpdateAngDecay();
2732 break;
2733 case Vehicle.LINEAR_FRICTION_TIMESCALE:
2734 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
2735 break;
2736 case Vehicle.LINEAR_MOTOR_DIRECTION:
2737 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
2738 UpdateLinDecay();
2739 break;
2740 case Vehicle.LINEAR_MOTOR_OFFSET:
2741 // m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
2742 break;
2743
2744 }
2745
2746 }//end ProcessFloatVehicleParam
2747
2748 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
2749 {
2750 switch (pParam)
2751 {
2752 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
2753 if (pValue.X > 30f) pValue.X = 30f;
2754 if (pValue.X < 0.1f) pValue.X = 0.1f;
2755 if (pValue.Y > 30f) pValue.Y = 30f;
2756 if (pValue.Y < 0.1f) pValue.Y = 0.1f;
2757 if (pValue.Z > 30f) pValue.Z = 30f;
2758 if (pValue.Z < 0.1f) pValue.Z = 0.1f;
2759 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
2760 break;
2761 case Vehicle.ANGULAR_MOTOR_DIRECTION:
2762 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
2763 // Limit requested angular speed to 2 rps= 4 pi rads/sec
2764 if(m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f;
2765 if(m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f;
2766 if(m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f;
2767 if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f;
2768 if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f;
2769 if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f;
2770 UpdateAngDecay();
2771 break;
2772 case Vehicle.LINEAR_FRICTION_TIMESCALE:
2773 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
2774 break;
2775 case Vehicle.LINEAR_MOTOR_DIRECTION:
2776 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); // velocity requested by LSL, for max limiting
2777 UpdateLinDecay();
2778 break;
2779 case Vehicle.LINEAR_MOTOR_OFFSET:
2780 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
2781 break;
2782 }
2783
2784 }//end ProcessVectorVehicleParam
2785
2786 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
2787 {
2788 switch (pParam)
2789 {
2790 case Vehicle.REFERENCE_FRAME:
2791 // m_referenceFrame = pValue;
2792 break;
2793 }
2794
2795 }//end ProcessRotationVehicleParam
2796
2797 internal void ProcessVehicleFlags(int pParam, bool remove)
2798 {
2799 if (remove)
2800 {
2801 m_flags &= ~((VehicleFlag)pParam);
2802 }
2803 else
2804 {
2805 m_flags |= (VehicleFlag)pParam;
2806 }
2807 }
2808
2809 internal void ProcessTypeChange(Vehicle pType)
2810 {
2811 // Set Defaults For Type
2812 m_type = pType;
2813 switch (pType)
2814 {
2815 case Vehicle.TYPE_SLED:
2816 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
2817 m_angularFrictionTimescale = new Vector3(30, 30, 30);
2818// m_lLinMotorVel = Vector3.Zero;
2819 m_linearMotorTimescale = 1000;
2820 m_linearMotorDecayTimescale = 120;
2821 m_angularMotorDirection = Vector3.Zero;
2822 m_angularMotorDVel = Vector3.Zero;
2823 m_angularMotorTimescale = 1000;
2824 m_angularMotorDecayTimescale = 120;
2825 m_VhoverHeight = 0;
2826// m_VhoverEfficiency = 1;
2827 m_VhoverTimescale = 10;
2828 m_VehicleBuoyancy = 0;
2829 // m_linearDeflectionEfficiency = 1;
2830 // m_linearDeflectionTimescale = 1;
2831 // m_angularDeflectionEfficiency = 1;
2832 // m_angularDeflectionTimescale = 1000;
2833 // m_bankingEfficiency = 0;
2834 // m_bankingMix = 1;
2835 // m_bankingTimescale = 10;
2836 // m_referenceFrame = Quaternion.Identity;
2837 m_flags &=
2838 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
2839 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
2840 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
2841 break;
2842 case Vehicle.TYPE_CAR:
2843 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
2844 m_angularFrictionTimescale = new Vector3(30, 30, 30); // was 1000, but sl max frict time is 30.
2845// m_lLinMotorVel = Vector3.Zero;
2846 m_linearMotorTimescale = 1;
2847 m_linearMotorDecayTimescale = 60;
2848 m_angularMotorDirection = Vector3.Zero;
2849 m_angularMotorDVel = Vector3.Zero;
2850 m_angularMotorTimescale = 1;
2851 m_angularMotorDecayTimescale = 0.8f;
2852 m_VhoverHeight = 0;
2853// m_VhoverEfficiency = 0;
2854 m_VhoverTimescale = 1000;
2855 m_VehicleBuoyancy = 0;
2856 // // m_linearDeflectionEfficiency = 1;
2857 // // m_linearDeflectionTimescale = 2;
2858 // // m_angularDeflectionEfficiency = 0;
2859 // m_angularDeflectionTimescale = 10;
2860 m_verticalAttractionEfficiency = 1f;
2861 m_verticalAttractionTimescale = 10f;
2862 // m_bankingEfficiency = -0.2f;
2863 // m_bankingMix = 1;
2864 // m_bankingTimescale = 1;
2865 // m_referenceFrame = Quaternion.Identity;
2866 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
2867 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY |
2868 VehicleFlag.LIMIT_MOTOR_UP);
2869 break;
2870 case Vehicle.TYPE_BOAT:
2871 m_linearFrictionTimescale = new Vector3(10, 3, 2);
2872 m_angularFrictionTimescale = new Vector3(10,10,10);
2873// m_lLinMotorVel = Vector3.Zero;
2874 m_linearMotorTimescale = 5;
2875 m_linearMotorDecayTimescale = 60;
2876 m_angularMotorDirection = Vector3.Zero;
2877 m_angularMotorDVel = Vector3.Zero;
2878 m_angularMotorTimescale = 4;
2879 m_angularMotorDecayTimescale = 4;
2880 m_VhoverHeight = 0;
2881// m_VhoverEfficiency = 0.5f;
2882 m_VhoverTimescale = 2;
2883 m_VehicleBuoyancy = 1;
2884 // m_linearDeflectionEfficiency = 0.5f;
2885 // m_linearDeflectionTimescale = 3;
2886 // m_angularDeflectionEfficiency = 0.5f;
2887 // m_angularDeflectionTimescale = 5;
2888 m_verticalAttractionEfficiency = 0.5f;
2889 m_verticalAttractionTimescale = 5f;
2890 // m_bankingEfficiency = -0.3f;
2891 // m_bankingMix = 0.8f;
2892 // m_bankingTimescale = 1;
2893 // m_referenceFrame = Quaternion.Identity;
2894 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY |
2895 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
2896 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY |
2897 VehicleFlag.LIMIT_MOTOR_UP);
2898 break;
2899 case Vehicle.TYPE_AIRPLANE:
2900 m_linearFrictionTimescale = new Vector3(200, 10, 5);
2901 m_angularFrictionTimescale = new Vector3(20, 20, 20);
2902// m_lLinMotorVel = Vector3.Zero;
2903 m_linearMotorTimescale = 2;
2904 m_linearMotorDecayTimescale = 60;
2905 m_angularMotorDirection = Vector3.Zero;
2906 m_angularMotorDVel = Vector3.Zero;
2907 m_angularMotorTimescale = 4;
2908 m_angularMotorDecayTimescale = 4;
2909 m_VhoverHeight = 0;
2910// m_VhoverEfficiency = 0.5f;
2911 m_VhoverTimescale = 1000;
2912 m_VehicleBuoyancy = 0;
2913 // m_linearDeflectionEfficiency = 0.5f;
2914 // m_linearDeflectionTimescale = 3;
2915 // m_angularDeflectionEfficiency = 1;
2916 // m_angularDeflectionTimescale = 2;
2917 m_verticalAttractionEfficiency = 0.9f;
2918 m_verticalAttractionTimescale = 2f;
2919 // m_bankingEfficiency = 1;
2920 // m_bankingMix = 0.7f;
2921 // m_bankingTimescale = 2;
2922 // m_referenceFrame = Quaternion.Identity;
2923 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
2924 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
2925 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
2926 break;
2927 case Vehicle.TYPE_BALLOON:
2928 m_linearFrictionTimescale = new Vector3(5, 5, 5);
2929 m_angularFrictionTimescale = new Vector3(10, 10, 10);
2930 m_linearMotorTimescale = 5;
2931 m_linearMotorDecayTimescale = 60;
2932 m_angularMotorDirection = Vector3.Zero;
2933 m_angularMotorDVel = Vector3.Zero;
2934 m_angularMotorTimescale = 6;
2935 m_angularMotorDecayTimescale = 10;
2936 m_VhoverHeight = 5;
2937// m_VhoverEfficiency = 0.8f;
2938 m_VhoverTimescale = 10;
2939 m_VehicleBuoyancy = 1;
2940 // m_linearDeflectionEfficiency = 0;
2941 // m_linearDeflectionTimescale = 5;
2942 // m_angularDeflectionEfficiency = 0;
2943 // m_angularDeflectionTimescale = 5;
2944 m_verticalAttractionEfficiency = 1f;
2945 m_verticalAttractionTimescale = 100f;
2946 // m_bankingEfficiency = 0;
2947 // m_bankingMix = 0.7f;
2948 // m_bankingTimescale = 5;
2949 // m_referenceFrame = Quaternion.Identity;
2950 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
2951 VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
2952 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
2953 break;
2954
2955 }
2956 }//end SetDefaultsForType
2957
2958 internal void Enable(IntPtr pBody, OdeScene pParentScene)
2959 {
2960 if (m_type == Vehicle.TYPE_NONE)
2961 return;
2962
2963 m_body = pBody;
2964 }
2965
2966
2967 internal void Halt()
2968 { // Kill all motions, when non-physical
2969 m_linearMotorDirection = Vector3.Zero;
2970 m_lLinMotorDVel = Vector3.Zero;
2971 m_lLinObjectVel = Vector3.Zero;
2972 m_wLinObjectVel = Vector3.Zero;
2973 m_angularMotorDirection = Vector3.Zero;
2974 m_lastAngularVelocity = Vector3.Zero;
2975 m_angularMotorDVel = Vector3.Zero;
2976 }
2977
2978 private void UpdateLinDecay()
2979 {
2980 if (Math.Abs(m_linearMotorDirection.X) > Math.Abs(m_lLinMotorDVel.X)) m_lLinMotorDVel.X = m_linearMotorDirection.X;
2981 if (Math.Abs(m_linearMotorDirection.Y) > Math.Abs(m_lLinMotorDVel.Y)) m_lLinMotorDVel.Y = m_linearMotorDirection.Y;
2982 if (Math.Abs(m_linearMotorDirection.Z) > Math.Abs(m_lLinMotorDVel.Z)) m_lLinMotorDVel.Z = m_linearMotorDirection.Z;
2983 } // else let the motor decay on its own
2984
2985 private void UpdateAngDecay()
2986 {
2987 if (Math.Abs(m_angularMotorDirection.X) > Math.Abs(m_angularMotorDVel.X)) m_angularMotorDVel.X = m_angularMotorDirection.X;
2988 if (Math.Abs(m_angularMotorDirection.Y) > Math.Abs(m_angularMotorDVel.Y)) m_angularMotorDVel.Y = m_angularMotorDirection.Y;
2989 if (Math.Abs(m_angularMotorDirection.Z) > Math.Abs(m_angularMotorDVel.Z)) m_angularMotorDVel.Z = m_angularMotorDirection.Z;
2990 } // else let the motor decay on its own
2991
2992 public void Move(float timestep)
2993 {
2994 float fx = 0;
2995 float fy = 0;
2996 float fz = 0;
2997
2998 frcount++; // used to limit debug comment output
2999 if (frcount > 100)
3000 frcount = 0;
3001
3002 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
3003 {
3004
3005// Old public void UpdatePositionAndVelocity(), more accuratley calculated here
3006 bool lastZeroFlag = _zeroFlag; // was it stopped
3007 d.Vector3 vec = d.BodyGetPosition(Body);
3008 d.Quaternion ori = d.BodyGetQuaternion(Body);
3009 d.Vector3 vel = d.BodyGetLinearVel(Body);
3010 // d.Vector3 rotvel = d.BodyGetAngularVel(Body);
3011 d.Vector3 torque = d.BodyGetTorque(Body);
3012 _torque = new Vector3(torque.X, torque.Y, torque.Z);
3013 Vector3 l_position = Vector3.Zero;
3014 Quaternion l_orientation = Quaternion.Identity;
3015
3016 m_lastposition = _position;
3017 m_lastorientation = _orientation;
3018
3019 l_position.X = vec.X;
3020 l_position.Y = vec.Y;
3021 l_position.Z = vec.Z;
3022 l_orientation.X = ori.X;
3023 l_orientation.Y = ori.Y;
3024 l_orientation.Z = ori.Z;
3025 l_orientation.W = ori.W;
3026//Console.WriteLine("Move {0} at {1}", m_primName, l_position);
3027
3028 // Check if outside region horizontally
3029 if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) ||
3030 l_position.X < 0f ||
3031 l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) ||
3032 l_position.Y < 0f)
3033 {
3034 if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
3035 { // keep trying to cross?
3036 _position = l_position;
3037 //_parent_scene.remActivePrim(this);
3038 if (_parent == null) base.RequestPhysicsterseUpdate();
3039 return; // Dont process any other motion?
3040 }
3041 else
3042 { // Too many tries
3043 if (_parent == null) base.RaiseOutOfBounds(l_position);
3044 return; // Dont process any other motion?
3045 }
3046 } // end outside region horizontally
3047
3048 if (l_position.Z < 0)
3049 {
3050 // This is so prim that get lost underground don't fall forever and suck up
3051 //
3052 // Sim resources and memory.
3053 // Disables the prim's movement physics....
3054 // It's a hack and will generate a console message if it fails.
3055
3056 //IsPhysical = false;
3057 if (_parent == null) base.RaiseOutOfBounds(_position);
3058
3059 _acceleration.X = 0; // This stuff may stop client display but it has no
3060 _acceleration.Y = 0; // effect on the object in phys engine!
3061 _acceleration.Z = 0;
3062
3063 _velocity.X = 0;
3064 _velocity.Y = 0;
3065 _velocity.Z = 0;
3066 m_rotationalVelocity.X = 0;
3067 m_rotationalVelocity.Y = 0;
3068 m_rotationalVelocity.Z = 0;
3069
3070 if (_parent == null) base.RequestPhysicsterseUpdate();
3071
3072 m_throttleUpdates = false;
3073 throttleCounter = 0;
3074 _zeroFlag = true;
3075 //outofBounds = true;
3076 } // end neg Z check
3077
3078 // Is it moving?
3079 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
3080 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
3081 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
3082 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large
3083 {
3084 _zeroFlag = true;
3085//Console.WriteLine("ZFT 2");
3086 m_throttleUpdates = false;
3087 }
3088 else
3089 {
3090 //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
3091 _zeroFlag = false;
3092 m_lastUpdateSent = false;
3093 //m_throttleUpdates = false;
3094 }
3095
3096 if (_zeroFlag)
3097 { // Its stopped
3098 _velocity.X = 0.0f;
3099 _velocity.Y = 0.0f;
3100 _velocity.Z = 0.0f;
3101
3102 _acceleration.X = 0;
3103 _acceleration.Y = 0;
3104 _acceleration.Z = 0;
3105 //_orientation.w = 0f;
3106 //_orientation.X = 0f;
3107 //_orientation.Y = 0f;
3108 //_orientation.Z = 0f;
3109 m_rotationalVelocity.X = 0;
3110 m_rotationalVelocity.Y = 0;
3111 m_rotationalVelocity.Z = 0;
3112 if (!m_lastUpdateSent)
3113 {
3114 m_throttleUpdates = false;
3115 throttleCounter = 0;
3116 if (_parent == null)
3117 {
3118 base.RequestPhysicsterseUpdate();
3119 }
3120
3121 m_lastUpdateSent = true;
3122 }
3123 }
3124 else
3125 { // Its moving
3126 if (lastZeroFlag != _zeroFlag)
3127 {
3128 if (_parent == null)
3129 {
3130 base.RequestPhysicsterseUpdate();
3131 }
3132 }
3133
3134 m_lastVelocity = _velocity;
3135
3136 _position = l_position;
3137
3138 _velocity.X = vel.X;
3139 _velocity.Y = vel.Y;
3140 _velocity.Z = vel.Z;
3141// Why 2 calcs???
3142// _acceleration = ((_velocity - m_lastVelocity) / 0.1f);
3143// _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f,
3144// _velocity.Y - m_lastVelocity.Y / 0.1f,
3145// _velocity.Z - m_lastVelocity.Z / 0.1f);
3146
3147 _acceleration = ((_velocity - m_lastVelocity) / timestep);
3148
3149 _orientation.X = ori.X;
3150 _orientation.Y = ori.Y;
3151 _orientation.Z = ori.Z;
3152 _orientation.W = ori.W;
3153 m_lastUpdateSent = false;
3154 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
3155 {
3156 if (_parent == null)
3157 {
3158 base.RequestPhysicsterseUpdate();
3159 }
3160 }
3161 else
3162 {
3163 throttleCounter++;
3164 }
3165 }
3166 m_lastposition = l_position;
3167
3168 /// End of old UpdatePositionAndVelocity insert
3169
3170//if (!Acceleration.ApproxEquals(Vector3.Zero, 0.01f)) Console.WriteLine("Move " + m_primName + " Accel=" + Acceleration);
3171// if(frcount == 0) Console.WriteLine("Move " + m_primName + " VTyp " + m_type +
3172// " usePID=" + m_usePID + " seHover=" + m_useHoverPID + " useAPID=" + m_useAPID);
3173 if (m_type != Vehicle.TYPE_NONE)
3174 {
3175 // get body attitude
3176 d.Quaternion rot = d.BodyGetQuaternion(Body);
3177 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
3178 Quaternion irotq = Quaternion.Inverse(rotq);
3179
3180 // VEHICLE Linear Motion
3181 d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame
3182 Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z);
3183 m_lLinObjectVel = vel_now * irotq;
3184
3185 if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate
3186 {
3187 if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f)
3188 {
3189 float decayfactor = m_linearMotorDecayTimescale/timestep;
3190 Vector3 decayAmount = (m_lLinMotorDVel/decayfactor);
3191 m_lLinMotorDVel -= decayAmount;
3192 }
3193 else
3194 {
3195 float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale)));
3196 Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep;
3197 m_lLinMotorDVel -= decel;
3198 }
3199 if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
3200 {
3201 m_lLinMotorDVel = Vector3.Zero;
3202 }
3203 else
3204 {
3205 if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X;
3206 if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y;
3207 if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z;
3208 }
3209 } // end linear motor decay
3210
3211 if ( (! m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
3212 {
3213 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
3214 if (m_linearMotorTimescale < 300.0f)
3215 {
3216 Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel;
3217 float linfactor = m_linearMotorTimescale/timestep;
3218 Vector3 attackAmount = (attack_error/linfactor) * 1.3f;
3219 m_lLinObjectVel += attackAmount;
3220 }
3221 if (m_linearFrictionTimescale.X < 300.0f)
3222 {
3223 float fricfactor = m_linearFrictionTimescale.X / timestep;
3224 float fricX = m_lLinObjectVel.X / fricfactor;
3225 m_lLinObjectVel.X -= fricX;
3226 }
3227 if (m_linearFrictionTimescale.Y < 300.0f)
3228 {
3229 float fricfactor = m_linearFrictionTimescale.Y / timestep;
3230 float fricY = m_lLinObjectVel.Y / fricfactor;
3231 m_lLinObjectVel.Y -= fricY;
3232 }
3233 if (m_linearFrictionTimescale.Z < 300.0f)
3234 {
3235 float fricfactor = m_linearFrictionTimescale.Z / timestep;
3236//if(frcount == 0) Console.WriteLine("Zfric={0}", fricfactor);
3237 float fricZ = m_lLinObjectVel.Z / fricfactor;
3238 m_lLinObjectVel.Z -= fricZ;
3239 }
3240 }
3241 m_wLinObjectVel = m_lLinObjectVel * rotq;
3242
3243 // Gravity and Buoyancy
3244 Vector3 grav = Vector3.Zero;
3245 if(m_VehicleBuoyancy < 1.0f)
3246 {
3247 // There is some gravity, make a gravity force vector
3248 // that is applied after object velocity.
3249 d.Mass objMass;
3250 d.BodyGetMass(Body, out objMass);
3251 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
3252 grav.Z = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force
3253 } // else its 1.0, no gravity.
3254
3255 // Hovering
3256 if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
3257 {
3258 // We should hover, get the target height
3259 d.Vector3 pos = d.BodyGetPosition(Body);
3260 if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
3261 {
3262 m_VhoverTargetHeight = _parent_scene.GetWaterLevel() + m_VhoverHeight;
3263 }
3264 else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
3265 {
3266 m_VhoverTargetHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
3267 }
3268 else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
3269 {
3270 m_VhoverTargetHeight = m_VhoverHeight;
3271 }
3272
3273 if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
3274 {
3275 // If body is aready heigher, use its height as target height
3276 if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
3277 }
3278
3279// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
3280// m_VhoverTimescale = 0f; // time to acheive height
3281// timestep is time since last frame,in secs
3282 float herr0 = pos.Z - m_VhoverTargetHeight;
3283 // Replace Vertical speed with correction figure if significant
3284 if(Math.Abs(herr0) > 0.01f )
3285 {
3286 //? d.Mass objMass;
3287 //? d.BodyGetMass(Body, out objMass);
3288 m_wLinObjectVel.Z = - ( (herr0 * timestep * 50.0f) / m_VhoverTimescale);
3289 //KF: m_VhoverEfficiency is not yet implemented
3290 }
3291 else
3292 {
3293 m_wLinObjectVel.Z = 0f;
3294 }
3295 }
3296 else
3297 { // not hovering, Gravity rules
3298 m_wLinObjectVel.Z = vel_now.Z;
3299 }
3300
3301
3302 // Vehicle Linear Motion done =======================================
3303 // Apply velocity
3304 d.BodySetLinearVel(Body, m_wLinObjectVel.X, m_wLinObjectVel.Y, m_wLinObjectVel.Z);
3305 // apply gravity force
3306 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
3307//if(frcount == 0) Console.WriteLine("Grav {0}", grav);
3308 // end MoveLinear()
3309
3310
3311 // MoveAngular
3312 /*
3313 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
3314
3315 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
3316 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
3317 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
3318
3319 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
3320 private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body
3321 */
3322//if(frcount == 0) Console.WriteLine("MoveAngular ");
3323
3324 d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body);
3325 Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z);
3326 angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation
3327
3328//if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel);
3329
3330 // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack.
3331 float atk_decayfactor = 23.0f / (m_angularMotorTimescale * timestep);
3332 m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor;
3333 // Decay Angular Motor 2.
3334 if (m_angularMotorDecayTimescale < 300.0f)
3335 {
3336 if ( Vector3.Mag(m_angularMotorDVel) < 1.0f)
3337 {
3338 float decayfactor = (m_angularMotorDecayTimescale)/timestep;
3339 Vector3 decayAmount = (m_angularMotorDVel/decayfactor);
3340 m_angularMotorDVel -= decayAmount;
3341 }
3342 else
3343 {
3344 Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale;
3345 m_angularMotorDVel -= decel;
3346 }
3347
3348 if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
3349 {
3350 m_angularMotorDVel = Vector3.Zero;
3351 }
3352 else
3353 {
3354 if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X;
3355 if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y;
3356 if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z;
3357 }
3358 } // end decay angular motor
3359//if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel);
3360
3361//if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel);
3362
3363 // Vertical attractor section
3364 Vector3 vertattr = Vector3.Zero;
3365
3366 if(m_verticalAttractionTimescale < 300)
3367 {
3368 float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep);
3369 // make a vector pointing up
3370 Vector3 verterr = Vector3.Zero;
3371 verterr.Z = 1.0f;
3372 // rotate it to Body Angle
3373 verterr = verterr * rotq;
3374 // 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.
3375 // 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
3376 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
3377
3378 if (verterr.Z < 0.0f)
3379 { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to
3380 // vertical, BUT for some reason a z-rotation is imparted to the object. TBI.
3381//Console.WriteLine("InvertFlip");
3382 verterr.X = 2.0f - verterr.X;
3383 verterr.Y = 2.0f - verterr.Y;
3384 }
3385 verterr *= 0.5f;
3386 // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt)
3387
3388 if ((!angObjectVel.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f))
3389 {
3390 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
3391 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
3392 vertattr.X = verterr.Y;
3393 vertattr.Y = - verterr.X;
3394 vertattr.Z = 0f;
3395//if(frcount == 0) Console.WriteLine("VAerr=" + verterr);
3396
3397 // scaling appears better usingsquare-law
3398 float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
3399 float bounce = 1.0f - damped;
3400 // 0 = crit damp, 1 = bouncy
3401 float oavz = angObjectVel.Z; // retain z velocity
3402 // time-scaled correction, which sums, therefore is bouncy:
3403 angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce;
3404 // damped, good @ < 90:
3405 angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped);
3406 angObjectVel.Z = oavz;
3407//if(frcount == 0) Console.WriteLine("VA+");
3408//Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel);
3409 }
3410 else
3411 {
3412 // else error is very small
3413 angObjectVel.X = 0f;
3414 angObjectVel.Y = 0f;
3415//if(frcount == 0) Console.WriteLine("VA0");
3416 }
3417 } // else vertical attractor is off
3418//if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel);
3419
3420 if ( (! m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
3421 { // if motor or object have motion
3422 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
3423
3424 if (m_angularMotorTimescale < 300.0f)
3425 {
3426 Vector3 attack_error = m_angularMotorDVel - angObjectVel;
3427 float angfactor = m_angularMotorTimescale/timestep;
3428 Vector3 attackAmount = (attack_error/angfactor);
3429 angObjectVel += attackAmount;
3430//if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount);
3431//if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel);
3432 }
3433
3434 angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep);
3435 angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep);
3436 angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep);
3437 } // else no signif. motion
3438
3439//if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel);
3440 // Bank section tba
3441 // Deflection section tba
3442//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel);
3443
3444 m_lastAngularVelocity = angObjectVel;
3445
3446 if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f))
3447 {
3448 if (m_angularEnable.X == 0)
3449 m_lastAngularVelocity.X = 0f;
3450 if (m_angularEnable.Y == 0)
3451 m_lastAngularVelocity.Y = 0f;
3452 if (m_angularEnable.Z == 0)
3453 m_lastAngularVelocity.Z = 0f;
3454 }
3455 // Apply to the body
3456// Vector3 aInc = m_lastAngularVelocity - initavel;
3457//if(frcount == 0) Console.WriteLine("Inc {0}", aInc);
3458 m_lastAngularVelocity = m_lastAngularVelocity * rotq; // ================ Converts to WORLD rotation
3459
3460 d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
3461//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity);
3462
3463 } // end VEHICLES
3464 else
3465 {
3466 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
3467 // NON-'VEHICLES' are dealt with here
3468 /// Dynamics Angular Lock ========================================================================
3469 if (d.BodyIsEnabled(Body) && !m_angularEnable.ApproxEquals(Vector3.One, 0.003f))
3470 {
3471 d.Vector3 avel2 = d.BodyGetAngularVel(Body);
3472 if (m_angularEnable.X == 0)
3473 avel2.X = 0;
3474 if (m_angularEnable.Y == 0)
3475 avel2.Y = 0;
3476 if (m_angularEnable.Z == 0)
3477 avel2.Z = 0;
3478 d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
3479 }
3480
3481
3482 /// Dynamics Buoyancy ===============================================================================
3483 //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle.
3484 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
3485 // NB Prims in ODE are no subject to global gravity
3486 float m_mass = CalculateMass();
3487 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass
3488
3489 if (m_usePID)
3490 {
3491//if(frcount == 0) Console.WriteLine("PID " + m_primName);
3492 // KF - this is for object MoveToTarget.
3493
3494 //if (!d.BodyIsEnabled(Body))
3495 //d.BodySetForce(Body, 0f, 0f, 0f);
3496
3497 // no lock; for now it's only called from within Simulate()
3498
3499 // If the PID Controller isn't active then we set our force
3500 // calculating base velocity to the current position
3501
3502 if ((m_PIDTau < 1) && (m_PIDTau != 0))
3503 {
3504 //PID_G = PID_G / m_PIDTau;
3505 m_PIDTau = 1;
3506 }
3507
3508 if ((PID_G - m_PIDTau) <= 0)
3509 {
3510 PID_G = m_PIDTau + 1;
3511 }
3512 //PidStatus = true;
3513
3514 // PhysicsVector vec = new PhysicsVector();
3515// d.Vector3 vel = d.BodyGetLinearVel(Body);
3516
3517 d.Vector3 pos = d.BodyGetPosition(Body);
3518 _target_velocity =
3519 new Vector3(
3520 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
3521 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
3522 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
3523 );
3524
3525 // if velocity is zero, use position control; otherwise, velocity control
3526
3527 if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
3528 {
3529 // keep track of where we stopped. No more slippin' & slidin'
3530
3531 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3532 // react to the physics scene by moving it's position.
3533 // Avatar to Avatar collisions
3534 // Prim to avatar collisions
3535
3536 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
3537 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
3538 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
3539 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3540 d.BodySetLinearVel(Body, 0, 0, 0);
3541 d.BodyAddForce(Body, 0, 0, fz);
3542 // return;
3543 }
3544 else
3545 {
3546 _zeroFlag = false;
3547
3548 // We're flying and colliding with something
3549 fx = ((_target_velocity.X) - vel.X) * (PID_D);
3550 fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
3551
3552 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
3553
3554 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
3555 }
3556 } // end if (m_usePID)
3557
3558 /// Dynamics Hover ===================================================================================
3559 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
3560 if (m_useHoverPID && !m_usePID)
3561 {
3562//Console.WriteLine("Hover " + m_primName);
3563
3564 // If we're using the PID controller, then we have no gravity
3565 fz = (-1 * _parent_scene.gravityz) * m_mass;
3566
3567 // no lock; for now it's only called from within Simulate()
3568
3569 // If the PID Controller isn't active then we set our force
3570 // calculating base velocity to the current position
3571
3572 if ((m_PIDTau < 1))
3573 {
3574 PID_G = PID_G / m_PIDTau;
3575 }
3576
3577 if ((PID_G - m_PIDTau) <= 0)
3578 {
3579 PID_G = m_PIDTau + 1;
3580 }
3581
3582
3583 // Where are we, and where are we headed?
3584 d.Vector3 pos = d.BodyGetPosition(Body);
3585// d.Vector3 vel = d.BodyGetLinearVel(Body);
3586
3587
3588 // Non-Vehicles have a limited set of Hover options.
3589 // determine what our target height really is based on HoverType
3590 switch (m_PIDHoverType)
3591 {
3592 case PIDHoverType.Ground:
3593 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3594 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3595 break;
3596 case PIDHoverType.GroundAndWater:
3597 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3598 m_waterHeight = _parent_scene.GetWaterLevel();
3599 if (m_groundHeight > m_waterHeight)
3600 {
3601 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3602 }
3603 else
3604 {
3605 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
3606 }
3607 break;
3608
3609 } // end switch (m_PIDHoverType)
3610
3611
3612 _target_velocity =
3613 new Vector3(0.0f, 0.0f,
3614 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
3615 );
3616
3617 // if velocity is zero, use position control; otherwise, velocity control
3618
3619 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
3620 {
3621 // keep track of where we stopped. No more slippin' & slidin'
3622
3623 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3624 // react to the physics scene by moving it's position.
3625 // Avatar to Avatar collisions
3626 // Prim to avatar collisions
3627
3628 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
3629 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
3630 d.BodyAddForce(Body, 0, 0, fz);
3631 //KF this prevents furthur motions return;
3632 }
3633 else
3634 {
3635 _zeroFlag = false;
3636
3637 // We're flying and colliding with something
3638 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
3639 }
3640 } // end m_useHoverPID && !m_usePID
3641
3642 /// Dynamics RotLookAt =================================================================================
3643 if (m_useAPID)
3644 {
3645 // RotLookAt, apparently overrides all other rotation sources. Inputs:
3646 // Quaternion m_APIDTarget
3647 // float m_APIDStrength // From SL experiments, this is the time to get there
3648 // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
3649 // Also in SL the mass of the object has no effect on time to get there.
3650 // Factors:
3651 // get present body rotation
3652 float limit = 1.0f;
3653 float scaler = 50f; // adjusts damping time
3654 float RLAservo = 0f;
3655
3656 d.Quaternion rot = d.BodyGetQuaternion(Body);
3657 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
3658 Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget;
3659 float diff_angle;
3660 Vector3 diff_axis;
3661 rot_diff.GetAxisAngle(out diff_axis, out diff_angle);
3662 diff_axis.Normalize();
3663 if(diff_angle > 0.01f) // diff_angle is always +ve
3664 {
3665// PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z);
3666 Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
3667 rotforce = rotforce * rotq;
3668 if(diff_angle > limit) diff_angle = limit; // cap the rotate rate
3669// RLAservo = timestep / m_APIDStrength * m_mass * scaler;
3670 // rotforce = rotforce * RLAservo * diff_angle ;
3671 // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z);
3672 RLAservo = timestep / m_APIDStrength * scaler;
3673 rotforce = rotforce * RLAservo * diff_angle ;
3674
3675 if (m_angularEnable.X == 0)
3676 rotforce.X = 0;
3677 if (m_angularEnable.Y == 0)
3678 rotforce.Y = 0;
3679 if (m_angularEnable.Z == 0)
3680 rotforce.Z = 0;
3681
3682 d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z);
3683//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo);
3684 }
3685//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle);
3686 } // end m_useAPID
3687
3688 /// Dynamics Apply Forces ===================================================================================
3689 fx *= m_mass;
3690 fy *= m_mass;
3691 //fz *= m_mass;
3692
3693 fx += m_force.X;
3694 fy += m_force.Y;
3695 fz += m_force.Z;
3696
3697 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
3698 if (fx != 0 || fy != 0 || fz != 0)
3699 {
3700 //m_taintdisable = true;
3701 //base.RaiseOutOfBounds(Position);
3702 //d.BodySetLinearVel(Body, fx, fy, 0f);
3703 if (!d.BodyIsEnabled(Body))
3704 {
3705 // A physical body at rest on a surface will auto-disable after a while,
3706 // this appears to re-enable it incase the surface it is upon vanishes,
3707 // and the body should fall again.
3708 d.BodySetLinearVel(Body, 0f, 0f, 0f);
3709 d.BodySetForce(Body, 0, 0, 0);
3710 enableBodySoft();
3711 }
3712
3713 // 35x10 = 350n times the mass per second applied maximum.
3714 float nmax = 35f * m_mass;
3715 float nmin = -35f * m_mass;
3716
3717
3718 if (fx > nmax)
3719 fx = nmax;
3720 if (fx < nmin)
3721 fx = nmin;
3722 if (fy > nmax)
3723 fy = nmax;
3724 if (fy < nmin)
3725 fy = nmin;
3726 d.BodyAddForce(Body, fx, fy, fz);
3727//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
3728 }
3729 }
3730 }
3731 else
3732 { // is not physical, or is not a body or is selected
3733 // from old UpdatePositionAndVelocity, ... Not a body.. so Make sure the client isn't interpolating
3734 _velocity.X = 0;
3735 _velocity.Y = 0;
3736 _velocity.Z = 0;
3737
3738 _acceleration.X = 0;
3739 _acceleration.Y = 0;
3740 _acceleration.Z = 0;
3741
3742 m_rotationalVelocity.X = 0;
3743 m_rotationalVelocity.Y = 0;
3744 m_rotationalVelocity.Z = 0;
3745 _zeroFlag = true;
3746 return;
3747 }
3748 } // end Move()
3749 } // end class
3750}
generated by cgit v1.1 at 2024-12-17 22:26:16 +0000