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