aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs')
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs3901
1 files changed, 3901 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..6b17ce7
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -0,0 +1,3901 @@
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_rotateEnable = Vector3.One; // Current setting
68 private Vector3 m_rotateEnableRequest = Vector3.One; // Request from LSL
69 private bool m_rotateEnableUpdate = false;
70 private Vector3 m_lockX;
71 private Vector3 m_lockY;
72 private Vector3 m_lockZ;
73 private IntPtr Amotor = IntPtr.Zero;
74 private IntPtr AmotorX = IntPtr.Zero;
75 private IntPtr AmotorY = IntPtr.Zero;
76 private IntPtr AmotorZ = IntPtr.Zero;
77
78 private Vector3 m_PIDTarget;
79 private float m_PIDTau;
80 private float PID_D = 35f;
81 private float PID_G = 25f;
82 private bool m_usePID = false;
83
84 private Quaternion m_APIDTarget = new Quaternion();
85 private float m_APIDStrength = 0.5f;
86 private float m_APIDDamping = 0.5f;
87 private bool m_useAPID = false;
88
89 // These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
90 // do not confuse with VEHICLE HOVER
91
92 private float m_PIDHoverHeight;
93 private float m_PIDHoverTau;
94 private bool m_useHoverPID;
95 private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
96 private float m_targetHoverHeight;
97 private float m_groundHeight;
98 private float m_waterHeight;
99 private float m_buoyancy; //m_buoyancy set by llSetBuoyancy()
100
101 // private float m_tensor = 5f;
102 private int body_autodisable_frames = 20;
103
104
105 private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
106 | CollisionCategories.Space
107 | CollisionCategories.Body
108 | CollisionCategories.Character
109 );
110 private bool m_taintshape;
111 private bool m_taintPhysics;
112 private bool m_collidesLand = true;
113 private bool m_collidesWater;
114 public bool m_returnCollisions;
115
116 // Default we're a Geometry
117 private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
118
119 // Default, Collide with Other Geometries, spaces and Bodies
120 private CollisionCategories m_collisionFlags = m_default_collisionFlags;
121
122 public bool m_taintremove;
123 public bool m_taintdisable;
124 public bool m_disabled;
125 public bool m_taintadd;
126 public bool m_taintselected;
127 public bool m_taintCollidesWater;
128
129 public uint m_localID;
130
131 //public GCHandle gc;
132 private CollisionLocker ode;
133
134 private bool m_taintforce = false;
135 private bool m_taintaddangularforce = false;
136 private Vector3 m_force;
137 private List<Vector3> m_forcelist = new List<Vector3>();
138 private List<Vector3> m_angularforcelist = new List<Vector3>();
139
140 private IMesh _mesh;
141 private PrimitiveBaseShape _pbs;
142 private OdeScene _parent_scene;
143 public IntPtr m_targetSpace = IntPtr.Zero;
144 public IntPtr prim_geom;
145// public IntPtr prev_geom;
146 public IntPtr _triMeshData;
147
148 private IntPtr _linkJointGroup = IntPtr.Zero;
149 private PhysicsActor _parent;
150 private PhysicsActor m_taintparent;
151
152 private List<OdePrim> childrenPrim = new List<OdePrim>();
153
154 private bool iscolliding;
155 private bool m_isphysical;
156 private bool m_isSelected;
157
158 internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
159
160 private bool m_throttleUpdates;
161 private int throttleCounter;
162 public int m_interpenetrationcount;
163 public float m_collisionscore;
164 public int m_roundsUnderMotionThreshold;
165 private int m_crossingfailures;
166
167 public bool outofBounds;
168 private float m_density = 10.000006836f; // Aluminum g/cm3;
169
170 public bool _zeroFlag; // if body has been stopped
171 private bool m_lastUpdateSent;
172
173 public IntPtr Body = IntPtr.Zero;
174 public String m_primName;
175 private Vector3 _target_velocity;
176 public d.Mass pMass;
177
178 public int m_eventsubscription;
179 private CollisionEventUpdate CollisionEventsThisFrame;
180
181 private IntPtr m_linkJoint = IntPtr.Zero;
182
183 public volatile bool childPrim;
184
185 internal int m_material = (int)Material.Wood;
186
187 private int frcount = 0; // Used to limit dynamics debug output to
188 private int revcount = 0; // Reverse motion while > 0
189
190 private IntPtr m_body = IntPtr.Zero;
191
192 // Vehicle properties ============================================================================================
193 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
194 // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
195 private VehicleFlag m_flags = (VehicleFlag) 0; // Bit settings:
196 // HOVER_TERRAIN_ONLY
197 // HOVER_GLOBAL_HEIGHT
198 // NO_DEFLECTION_UP
199 // HOVER_WATER_ONLY
200 // HOVER_UP_ONLY
201 // LIMIT_MOTOR_UP
202 // LIMIT_ROLL_ONLY
203
204 // Linear properties
205 private Vector3 m_linearMotorDirection = Vector3.Zero; // (was m_linearMotorDirectionLASTSET) the (local) Velocity
206 //requested by LSL
207 private float m_linearMotorTimescale = 0; // Motor Attack rate set by LSL
208 private float m_linearMotorDecayTimescale = 0; // Motor Decay rate set by LSL
209 private Vector3 m_linearFrictionTimescale = Vector3.Zero; // General Friction set by LSL
210
211 private Vector3 m_lLinMotorDVel = Vector3.Zero; // decayed motor
212 private Vector3 m_lLinObjectVel = Vector3.Zero; // local frame object velocity
213 private Vector3 m_wLinObjectVel = Vector3.Zero; // world frame object velocity
214
215 //Angular properties
216 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
217
218 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
219 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
220 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
221
222 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
223// private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity
224 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
225
226 //Deflection properties
227 // private float m_angularDeflectionEfficiency = 0;
228 // private float m_angularDeflectionTimescale = 0;
229 // private float m_linearDeflectionEfficiency = 0;
230 // private float m_linearDeflectionTimescale = 0;
231
232 //Banking properties
233 // private float m_bankingEfficiency = 0;
234 // private float m_bankingMix = 0;
235 // private float m_bankingTimescale = 0;
236
237 //Hover and Buoyancy properties
238 private float m_VhoverHeight = 0f;
239// private float m_VhoverEfficiency = 0f;
240 private float m_VhoverTimescale = 0f;
241 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
242 private float m_VehicleBuoyancy = 0f; // Set by VEHICLE_BUOYANCY, for a vehicle.
243 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
244 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
245 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
246
247 //Attractor properties
248 private float m_verticalAttractionEfficiency = 1.0f; // damped
249 private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor.
250
251
252
253
254
255
256 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
257 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode)
258 {
259 ode = dode;
260 if (!pos.IsFinite())
261 {
262 pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
263 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
264 m_log.Warn("[PHYSICS]: Got nonFinite Object create Position");
265 }
266
267 _position = pos;
268 m_taintposition = pos;
269 PID_D = parent_scene.bodyPIDD;
270 PID_G = parent_scene.bodyPIDG;
271 m_density = parent_scene.geomDefaultDensity;
272 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
273 body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
274
275
276 prim_geom = IntPtr.Zero;
277// prev_geom = IntPtr.Zero;
278
279 if (!pos.IsFinite())
280 {
281 size = new Vector3(0.5f, 0.5f, 0.5f);
282 m_log.Warn("[PHYSICS]: Got nonFinite Object create Size");
283 }
284
285 if (size.X <= 0) size.X = 0.01f;
286 if (size.Y <= 0) size.Y = 0.01f;
287 if (size.Z <= 0) size.Z = 0.01f;
288
289 _size = size;
290 m_taintsize = _size;
291
292 if (!QuaternionIsFinite(rotation))
293 {
294 rotation = Quaternion.Identity;
295 m_log.Warn("[PHYSICS]: Got nonFinite Object create Rotation");
296 }
297
298 _orientation = rotation;
299 m_taintrot = _orientation;
300 _mesh = mesh;
301 _pbs = pbs;
302
303 _parent_scene = parent_scene;
304 m_targetSpace = (IntPtr)0;
305
306// if (pos.Z < 0)
307 if (pos.Z < parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y))
308 m_isphysical = false;
309 else
310 {
311 m_isphysical = pisPhysical;
312 // If we're physical, we need to be in the master space for now.
313 // linksets *should* be in a space together.. but are not currently
314 if (m_isphysical)
315 m_targetSpace = _parent_scene.space;
316 }
317 m_primName = primName;
318 m_taintadd = true;
319 _parent_scene.AddPhysicsActorTaint(this);
320 // don't do .add() here; old geoms get recycled with the same hash
321 }
322
323 public override int PhysicsActorType
324 {
325 get { return (int) ActorTypes.Prim; }
326 set { return; }
327 }
328
329 public override bool SetAlwaysRun
330 {
331 get { return false; }
332 set { return; }
333 }
334
335 public override uint LocalID
336 {
337 set {
338 //m_log.Info("[PHYSICS]: Setting TrackerID: " + value);
339 m_localID = value; }
340 }
341
342 public override bool Grabbed
343 {
344 set { return; }
345 }
346
347 public override bool Selected
348 {
349 set {
350
351//Console.WriteLine("Sel {0} {1} {2}", m_primName, value, m_isphysical);
352 // This only makes the object not collidable if the object
353 // is physical or the object is modified somehow *IN THE FUTURE*
354 // without this, if an avatar selects prim, they can walk right
355 // through it while it's selected
356 m_collisionscore = 0;
357 if ((m_isphysical && !_zeroFlag) || !value)
358 {
359 m_taintselected = value;
360 _parent_scene.AddPhysicsActorTaint(this);
361 }
362 else
363 {
364 m_taintselected = value;
365 m_isSelected = value;
366 }
367 if(m_isSelected) disableBodySoft();
368 }
369 }
370
371 public override bool IsPhysical
372 {
373 get { return m_isphysical; }
374 set
375 {
376 m_isphysical = value;
377 if (!m_isphysical)
378 { // Zero the remembered last velocity
379 m_lastVelocity = Vector3.Zero;
380 if (m_type != Vehicle.TYPE_NONE) Halt();
381 }
382 }
383 }
384
385 public void setPrimForRemoval()
386 {
387 m_taintremove = true;
388 }
389
390 public override bool Flying
391 {
392 // no flying prims for you
393 get { return false; }
394 set { }
395 }
396
397 public override bool IsColliding
398 {
399 get { return iscolliding; }
400 set { iscolliding = value; }
401 }
402
403 public override bool CollidingGround
404 {
405 get { return false; }
406 set { return; }
407 }
408
409 public override bool CollidingObj
410 {
411 get { return false; }
412 set { return; }
413 }
414
415 public override bool ThrottleUpdates
416 {
417 get { return m_throttleUpdates; }
418 set { m_throttleUpdates = value; }
419 }
420
421 public override bool Stopped
422 {
423 get { return _zeroFlag; }
424 }
425
426 public override Vector3 Position
427 {
428 get { return _position; }
429
430 set { _position = value;
431 //m_log.Info("[PHYSICS]: " + _position.ToString());
432 }
433 }
434
435 public override Vector3 Size
436 {
437 get { return _size; }
438 set
439 {
440 if (value.IsFinite())
441 {
442 _size = value;
443 }
444 else
445 {
446 m_log.Warn("[PHYSICS]: Got NaN Size on object");
447 }
448 }
449 }
450
451 public override float Mass
452 {
453 get { return CalculateMass(); }
454 }
455
456 public override Vector3 Force
457 {
458 //get { return Vector3.Zero; }
459 get { return m_force; }
460 set
461 {
462 if (value.IsFinite())
463 {
464 m_force = value;
465 }
466 else
467 {
468 m_log.Warn("[PHYSICS]: NaN in Force Applied to an Object");
469 }
470 }
471 }
472
473 public override int VehicleType
474 {
475 get { return (int)m_type; }
476 set { ProcessTypeChange((Vehicle)value); }
477 }
478
479 public override void VehicleFloatParam(int param, float value)
480 {
481 ProcessFloatVehicleParam((Vehicle) param, value);
482 }
483
484 public override void VehicleVectorParam(int param, Vector3 value)
485 {
486 ProcessVectorVehicleParam((Vehicle) param, value);
487 }
488
489 public override void VehicleRotationParam(int param, Quaternion rotation)
490 {
491 ProcessRotationVehicleParam((Vehicle) param, rotation);
492 }
493
494 public override void VehicleFlags(int param, bool remove)
495 {
496 ProcessVehicleFlags(param, remove);
497 }
498
499 public override void SetVolumeDetect(int param)
500 {
501 lock (_parent_scene.OdeLock)
502 {
503 m_isVolumeDetect = (param!=0);
504 }
505 }
506
507 public override Vector3 CenterOfMass
508 {
509 get { return Vector3.Zero; }
510 }
511
512 public override Vector3 GeometricCenter
513 {
514 get { return Vector3.Zero; }
515 }
516
517 public override PrimitiveBaseShape Shape
518 {
519 set
520 {
521 _pbs = value;
522 m_taintshape = true;
523 }
524 }
525
526 public override Vector3 Velocity
527 {
528 get
529 {
530 // Averate previous velocity with the new one so
531 // client object interpolation works a 'little' better
532 if (_zeroFlag)
533 return Vector3.Zero;
534
535 Vector3 returnVelocity = Vector3.Zero;
536 returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2;
537 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2;
538 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2;
539 return returnVelocity;
540 }
541 set
542 {
543 if (value.IsFinite())
544 {
545 _velocity = value;
546
547 m_taintVelocity = value;
548 _parent_scene.AddPhysicsActorTaint(this);
549 }
550 else
551 {
552 m_log.Warn("[PHYSICS]: Got NaN Velocity in Object");
553 }
554
555 }
556 }
557
558 public override Vector3 Torque
559 {
560 get
561 {
562 if (!m_isphysical || Body == IntPtr.Zero)
563 return Vector3.Zero;
564
565 return _torque;
566 }
567
568 set
569 {
570 if (value.IsFinite())
571 {
572 m_taintTorque = value;
573 _parent_scene.AddPhysicsActorTaint(this);
574 }
575 else
576 {
577 m_log.Warn("[PHYSICS]: Got NaN Torque in Object");
578 }
579 }
580 }
581
582 public override float CollisionScore
583 {
584 get { return m_collisionscore; }
585 set { m_collisionscore = value; }
586 }
587
588 public override bool Kinematic
589 {
590 get { return false; }
591 set { }
592 }
593
594 public override Quaternion Orientation
595 {
596 get { return _orientation; }
597 set
598 {
599 if (QuaternionIsFinite(value))
600 {
601 _orientation = value;
602 }
603 else
604 m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object");
605
606 }
607 }
608
609
610 public override bool FloatOnWater
611 {
612 set {
613 m_taintCollidesWater = value;
614 _parent_scene.AddPhysicsActorTaint(this);
615 }
616 }
617
618 public override void SetMomentum(Vector3 momentum)
619 {
620 }
621
622 public override Vector3 PIDTarget
623 {
624 set
625 {
626 if (value.IsFinite())
627 {
628 m_PIDTarget = value;
629 }
630 else
631 m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object");
632 }
633 }
634 public override bool PIDActive { set { m_usePID = value; } }
635 public override float PIDTau { set { m_PIDTau = value; } }
636
637 // For RotLookAt
638 public override Quaternion APIDTarget { set { m_APIDTarget = value; } }
639 public override bool APIDActive { set { m_useAPID = value; } }
640 public override float APIDStrength { set { m_APIDStrength = value; } }
641 public override float APIDDamping { set { m_APIDDamping = value; } }
642
643 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
644 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
645 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
646 public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
647
648 internal static bool QuaternionIsFinite(Quaternion q)
649 {
650 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
651 return false;
652 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
653 return false;
654 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
655 return false;
656 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
657 return false;
658 return true;
659 }
660
661 public override Vector3 Acceleration // client updates read data via here
662 {
663 get { return _acceleration; }
664 }
665
666
667 public void SetAcceleration(Vector3 accel) // No one calls this, and it would not do anything.
668 {
669 _acceleration = accel;
670 }
671
672 public override void AddForce(Vector3 force, bool pushforce)
673 {
674 if (force.IsFinite())
675 {
676 lock (m_forcelist)
677 m_forcelist.Add(force);
678
679 m_taintforce = true;
680 }
681 else
682 {
683 m_log.Warn("[PHYSICS]: Got Invalid linear force vector from Scene in Object");
684 }
685 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
686 }
687
688 public override void AddAngularForce(Vector3 force, bool pushforce)
689 {
690 if (force.IsFinite())
691 {
692 m_angularforcelist.Add(force);
693 m_taintaddangularforce = true;
694 }
695 else
696 {
697 m_log.Warn("[PHYSICS]: Got Invalid Angular force vector from Scene in Object");
698 }
699 }
700
701 public override Vector3 RotationalVelocity
702 {
703 get
704 {
705 return m_rotationalVelocity;
706 }
707 set
708 {
709 if (value.IsFinite())
710 {
711 m_rotationalVelocity = value;
712 }
713 else
714 {
715 m_log.Warn("[PHYSICS]: Got NaN RotationalVelocity in Object");
716 }
717 }
718 }
719
720 public override void CrossingFailure()
721 {
722 m_crossingfailures++;
723 if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
724 {
725 base.RaiseOutOfBounds(_position);
726 return;
727 }
728 else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
729 {
730 m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);
731 }
732 }
733
734 public override float Buoyancy
735 {
736 get { return m_buoyancy; }
737// set { m_buoyancy = value; }
738 set {
739 m_buoyancy = value;
740
741 Console.WriteLine("m_buoyancy={0}", m_buoyancy);
742 }
743 }
744
745 public override void link(PhysicsActor obj)
746 {
747 m_taintparent = obj;
748 }
749
750 public override void delink()
751 {
752 m_taintparent = null;
753 }
754
755 public override void LockAngularMotion(Vector3 axis)
756 {
757 // This is actually ROTATION ENABLE, not a lock.
758 // default is <1,1,1> which is all enabled.
759 // The lock value is updated inside Move(), no point in using the taint system.
760 // OS 'm_taintAngularLock' etc change to m_rotateEnable.
761 if (axis.IsFinite())
762 {
763 axis.X = (axis.X > 0) ? 1f : 0f;
764 axis.Y = (axis.Y > 0) ? 1f : 0f;
765 axis.Z = (axis.Z > 0) ? 1f : 0f;
766 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
767 m_rotateEnableRequest = axis;
768 m_rotateEnableUpdate = true;
769 }
770 else
771 {
772 m_log.Warn("[PHYSICS]: Got NaN locking axis from Scene on Object");
773 }
774 }
775
776
777 public void SetGeom(IntPtr geom)
778 {
779 if(prim_geom != IntPtr.Zero)
780 {
781 // Remove any old entries
782//string tPA;
783//_parent_scene.geom_name_map.TryGetValue(prim_geom, out tPA);
784//Console.WriteLine("**** Remove {0}", tPA);
785 if(_parent_scene.geom_name_map.ContainsKey(prim_geom)) _parent_scene.geom_name_map.Remove(prim_geom);
786 if(_parent_scene.actor_name_map.ContainsKey(prim_geom)) _parent_scene.actor_name_map.Remove(prim_geom);
787 d.GeomDestroy(prim_geom);
788 }
789
790 prim_geom = geom;
791//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName);
792 if (prim_geom != IntPtr.Zero)
793 {
794 _parent_scene.geom_name_map[prim_geom] = this.m_primName;
795 _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
796 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
797 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
798//Console.WriteLine("**** Create {2} Dicts: actor={0} name={1}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, this.m_primName);
799 }
800
801 if (childPrim)
802 {
803 if (_parent != null && _parent is OdePrim)
804 {
805 OdePrim parent = (OdePrim)_parent;
806//Console.WriteLine("SetGeom calls ChildSetGeom");
807 parent.ChildSetGeom(this);
808 }
809 }
810 //m_log.Warn("Setting Geom to: " + prim_geom);
811 }
812
813 public void enableBodySoft()
814 {
815 if (!childPrim)
816 {
817 if (m_isphysical && Body != IntPtr.Zero)
818 {
819 d.BodyEnable(Body);
820 if (m_type != Vehicle.TYPE_NONE)
821 Enable(Body, _parent_scene);
822 }
823
824 m_disabled = false;
825 }
826 }
827
828 public void disableBodySoft()
829 {
830 m_disabled = true;
831
832 if (m_isphysical && Body != IntPtr.Zero)
833 {
834 d.BodyDisable(Body);
835 Halt();
836 }
837 }
838
839 public void enableBody()
840 {
841 // Don't enable this body if we're a child prim
842 // this should be taken care of in the parent function not here
843 if (!childPrim)
844 {
845 // Sets the geom to a body
846 Body = d.BodyCreate(_parent_scene.world);
847
848 setMass();
849 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
850 d.Quaternion myrot = new d.Quaternion();
851 myrot.X = _orientation.X;
852 myrot.Y = _orientation.Y;
853 myrot.Z = _orientation.Z;
854 myrot.W = _orientation.W;
855 d.BodySetQuaternion(Body, ref myrot);
856 d.GeomSetBody(prim_geom, Body);
857 m_collisionCategories |= CollisionCategories.Body;
858 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
859
860 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
861 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
862
863 d.BodySetAutoDisableFlag(Body, true);
864 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
865
866 // disconnect from world gravity so we can apply buoyancy
867 d.BodySetGravityMode (Body, false);
868
869 m_interpenetrationcount = 0;
870 m_collisionscore = 0;
871 m_disabled = false;
872
873 if (m_type != Vehicle.TYPE_NONE)
874 {
875 Enable(Body, _parent_scene);
876 }
877
878 _parent_scene.addActivePrim(this);
879 }
880 }
881
882 #region Mass Calculation
883
884 private float CalculateMass()
885 {
886 float volume = 0;
887
888 // No material is passed to the physics engines yet.. soo..
889 // we're using the m_density constant in the class definition
890
891 float returnMass = 0;
892
893 switch (_pbs.ProfileShape)
894 {
895 case ProfileShape.Square:
896 // Profile Volume
897
898 volume = _size.X*_size.Y*_size.Z;
899
900 // If the user has 'hollowed out'
901 // ProfileHollow is one of those 0 to 50000 values :P
902 // we like percentages better.. so turning into a percentage
903
904 if (((float) _pbs.ProfileHollow/50000f) > 0.0)
905 {
906 float hollowAmount = (float) _pbs.ProfileHollow/50000f;
907
908 // calculate the hollow volume by it's shape compared to the prim shape
909 float hollowVolume = 0;
910 switch (_pbs.HollowShape)
911 {
912 case HollowShape.Square:
913 case HollowShape.Same:
914 // Cube Hollow volume calculation
915 float hollowsizex = _size.X*hollowAmount;
916 float hollowsizey = _size.Y*hollowAmount;
917 float hollowsizez = _size.Z*hollowAmount;
918 hollowVolume = hollowsizex*hollowsizey*hollowsizez;
919 break;
920
921 case HollowShape.Circle:
922 // Hollow shape is a perfect cyllinder in respect to the cube's scale
923 // Cyllinder hollow volume calculation
924 float hRadius = _size.X/2;
925 float hLength = _size.Z;
926
927 // pi * r2 * h
928 hollowVolume = ((float) (Math.PI*Math.Pow(hRadius, 2)*hLength)*hollowAmount);
929 break;
930
931 case HollowShape.Triangle:
932 // Equilateral Triangular Prism volume hollow calculation
933 // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
934
935 float aLength = _size.Y;
936 // 1/2 abh
937 hollowVolume = (float) ((0.5*aLength*_size.X*_size.Z)*hollowAmount);
938 break;
939
940 default:
941 hollowVolume = 0;
942 break;
943 }
944 volume = volume - hollowVolume;
945 }
946
947 break;
948 case ProfileShape.Circle:
949 if (_pbs.PathCurve == (byte)Extrusion.Straight)
950 {
951 // Cylinder
952 float volume1 = (float)(Math.PI * Math.Pow(_size.X/2, 2) * _size.Z);
953 float volume2 = (float)(Math.PI * Math.Pow(_size.Y/2, 2) * _size.Z);
954
955 // Approximating the cylinder's irregularity.
956 if (volume1 > volume2)
957 {
958 volume = (float)volume1 - (volume1 - volume2);
959 }
960 else if (volume2 > volume1)
961 {
962 volume = (float)volume2 - (volume2 - volume1);
963 }
964 else
965 {
966 // Regular cylinder
967 volume = volume1;
968 }
969 }
970 else
971 {
972 // We don't know what the shape is yet, so use default
973 volume = _size.X * _size.Y * _size.Z;
974 }
975 // If the user has 'hollowed out'
976 // ProfileHollow is one of those 0 to 50000 values :P
977 // we like percentages better.. so turning into a percentage
978
979 if (((float)_pbs.ProfileHollow / 50000f) > 0.0)
980 {
981 float hollowAmount = (float)_pbs.ProfileHollow / 50000f;
982
983 // calculate the hollow volume by it's shape compared to the prim shape
984 float hollowVolume = 0;
985 switch (_pbs.HollowShape)
986 {
987 case HollowShape.Same:
988 case HollowShape.Circle:
989 // Hollow shape is a perfect cyllinder in respect to the cube's scale
990 // Cyllinder hollow volume calculation
991 float hRadius = _size.X / 2;
992 float hLength = _size.Z;
993
994 // pi * r2 * h
995 hollowVolume = ((float)(Math.PI * Math.Pow(hRadius, 2) * hLength) * hollowAmount);
996 break;
997
998 case HollowShape.Square:
999 // Cube Hollow volume calculation
1000 float hollowsizex = _size.X * hollowAmount;
1001 float hollowsizey = _size.Y * hollowAmount;
1002 float hollowsizez = _size.Z * hollowAmount;
1003 hollowVolume = hollowsizex * hollowsizey * hollowsizez;
1004 break;
1005
1006 case HollowShape.Triangle:
1007 // Equilateral Triangular Prism volume hollow calculation
1008 // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
1009
1010 float aLength = _size.Y;
1011 // 1/2 abh
1012 hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);
1013 break;
1014
1015 default:
1016 hollowVolume = 0;
1017 break;
1018 }
1019 volume = volume - hollowVolume;
1020 }
1021 break;
1022
1023 case ProfileShape.HalfCircle:
1024 if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1025 {
1026 if (_size.X == _size.Y && _size.Y == _size.Z)
1027 {
1028 // regular sphere
1029 // v = 4/3 * pi * r^3
1030 float sradius3 = (float)Math.Pow((_size.X / 2), 3);
1031 volume = (float)((4f / 3f) * Math.PI * sradius3);
1032 }
1033 else
1034 {
1035 // we treat this as a box currently
1036 volume = _size.X * _size.Y * _size.Z;
1037 }
1038 }
1039 else
1040 {
1041 // We don't know what the shape is yet, so use default
1042 volume = _size.X * _size.Y * _size.Z;
1043 }
1044 break;
1045
1046 case ProfileShape.EquilateralTriangle:
1047 /*
1048 v = (abs((xB*yA-xA*yB)+(xC*yB-xB*yC)+(xA*yC-xC*yA))/2) * h
1049
1050 // seed mesh
1051 Vertex MM = new Vertex(-0.25f, -0.45f, 0.0f);
1052 Vertex PM = new Vertex(+0.5f, 0f, 0.0f);
1053 Vertex PP = new Vertex(-0.25f, +0.45f, 0.0f);
1054 */
1055 float xA = -0.25f * _size.X;
1056 float yA = -0.45f * _size.Y;
1057
1058 float xB = 0.5f * _size.X;
1059 float yB = 0;
1060
1061 float xC = -0.25f * _size.X;
1062 float yC = 0.45f * _size.Y;
1063
1064 volume = (float)((Math.Abs((xB * yA - xA * yB) + (xC * yB - xB * yC) + (xA * yC - xC * yA)) / 2) * _size.Z);
1065
1066 // If the user has 'hollowed out'
1067 // ProfileHollow is one of those 0 to 50000 values :P
1068 // we like percentages better.. so turning into a percentage
1069 float fhollowFactor = ((float)_pbs.ProfileHollow / 1.9f);
1070 if (((float)fhollowFactor / 50000f) > 0.0)
1071 {
1072 float hollowAmount = (float)fhollowFactor / 50000f;
1073
1074 // calculate the hollow volume by it's shape compared to the prim shape
1075 float hollowVolume = 0;
1076 switch (_pbs.HollowShape)
1077 {
1078 case HollowShape.Same:
1079 case HollowShape.Triangle:
1080 // Equilateral Triangular Prism volume hollow calculation
1081 // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
1082
1083 float aLength = _size.Y;
1084 // 1/2 abh
1085 hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);
1086 break;
1087
1088 case HollowShape.Square:
1089 // Cube Hollow volume calculation
1090 float hollowsizex = _size.X * hollowAmount;
1091 float hollowsizey = _size.Y * hollowAmount;
1092 float hollowsizez = _size.Z * hollowAmount;
1093 hollowVolume = hollowsizex * hollowsizey * hollowsizez;
1094 break;
1095
1096 case HollowShape.Circle:
1097 // Hollow shape is a perfect cyllinder in respect to the cube's scale
1098 // Cyllinder hollow volume calculation
1099 float hRadius = _size.X / 2;
1100 float hLength = _size.Z;
1101
1102 // pi * r2 * h
1103 hollowVolume = ((float)((Math.PI * Math.Pow(hRadius, 2) * hLength)/2) * hollowAmount);
1104 break;
1105
1106 default:
1107 hollowVolume = 0;
1108 break;
1109 }
1110 volume = volume - hollowVolume;
1111 }
1112 break;
1113
1114 default:
1115 // we don't have all of the volume formulas yet so
1116 // use the common volume formula for all
1117 volume = _size.X*_size.Y*_size.Z;
1118 break;
1119 }
1120
1121 // Calculate Path cut effect on volume
1122 // Not exact, in the triangle hollow example
1123 // They should never be zero or less then zero..
1124 // we'll ignore it if it's less then zero
1125
1126 // ProfileEnd and ProfileBegin are values
1127 // from 0 to 50000
1128
1129 // Turning them back into percentages so that I can cut that percentage off the volume
1130
1131 float PathCutEndAmount = _pbs.ProfileEnd;
1132 float PathCutStartAmount = _pbs.ProfileBegin;
1133 if (((PathCutStartAmount + PathCutEndAmount)/50000f) > 0.0f)
1134 {
1135 float pathCutAmount = ((PathCutStartAmount + PathCutEndAmount)/50000f);
1136
1137 // Check the return amount for sanity
1138 if (pathCutAmount >= 0.99f)
1139 pathCutAmount = 0.99f;
1140
1141 volume = volume - (volume*pathCutAmount);
1142 }
1143 UInt16 taperX = _pbs.PathScaleX;
1144 UInt16 taperY = _pbs.PathScaleY;
1145 float taperFactorX = 0;
1146 float taperFactorY = 0;
1147
1148 // Mass = density * volume
1149 if (taperX != 100)
1150 {
1151 if (taperX > 100)
1152 {
1153 taperFactorX = 1.0f - ((float)taperX / 200);
1154 //m_log.Warn("taperTopFactorX: " + extr.taperTopFactorX.ToString());
1155 }
1156 else
1157 {
1158 taperFactorX = 1.0f - ((100 - (float)taperX) / 100);
1159 //m_log.Warn("taperBotFactorX: " + extr.taperBotFactorX.ToString());
1160 }
1161 volume = (float)volume * ((taperFactorX / 3f) + 0.001f);
1162 }
1163
1164 if (taperY != 100)
1165 {
1166 if (taperY > 100)
1167 {
1168 taperFactorY = 1.0f - ((float)taperY / 200);
1169 //m_log.Warn("taperTopFactorY: " + extr.taperTopFactorY.ToString());
1170 }
1171 else
1172 {
1173 taperFactorY = 1.0f - ((100 - (float)taperY) / 100);
1174 //m_log.Warn("taperBotFactorY: " + extr.taperBotFactorY.ToString());
1175 }
1176 volume = (float)volume * ((taperFactorY / 3f) + 0.001f);
1177 }
1178 returnMass = m_density*volume;
1179 if (returnMass <= 0) returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
1180
1181
1182
1183 // Recursively calculate mass
1184 bool HasChildPrim = false;
1185 lock (childrenPrim)
1186 {
1187 if (childrenPrim.Count > 0)
1188 {
1189 HasChildPrim = true;
1190 }
1191
1192 }
1193 if (HasChildPrim)
1194 {
1195 OdePrim[] childPrimArr = new OdePrim[0];
1196
1197 lock (childrenPrim)
1198 childPrimArr = childrenPrim.ToArray();
1199
1200 for (int i = 0; i < childPrimArr.Length; i++)
1201 {
1202 if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove)
1203 returnMass += childPrimArr[i].CalculateMass();
1204 // failsafe, this shouldn't happen but with OpenSim, you never know :)
1205 if (i > 256)
1206 break;
1207 }
1208 }
1209 if (returnMass > _parent_scene.maximumMassObject)
1210 returnMass = _parent_scene.maximumMassObject;
1211 return returnMass;
1212 }// end CalculateMass
1213
1214 #endregion
1215
1216 public void setMass()
1217 {
1218 if (Body != (IntPtr) 0)
1219 {
1220 float newmass = CalculateMass();
1221
1222 //m_log.Info("[PHYSICS]: New Mass: " + newmass.ToString());
1223
1224 d.MassSetBoxTotal(out pMass, newmass, _size.X, _size.Y, _size.Z);
1225 d.BodySetMass(Body, ref pMass);
1226 }
1227 }
1228
1229 public void disableBody()
1230 {
1231 //this kills the body so things like 'mesh' can re-create it.
1232 lock (this)
1233 {
1234 if (!childPrim)
1235 {
1236 if (Body != IntPtr.Zero)
1237 {
1238 _parent_scene.remActivePrim(this);
1239 m_collisionCategories &= ~CollisionCategories.Body;
1240 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
1241
1242 if (prim_geom != IntPtr.Zero)
1243 {
1244 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1245 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1246 }
1247
1248
1249 d.BodyDestroy(Body);
1250 lock (childrenPrim)
1251 {
1252 if (childrenPrim.Count > 0)
1253 {
1254 foreach (OdePrim prm in childrenPrim)
1255 {
1256 _parent_scene.remActivePrim(prm);
1257 prm.Body = IntPtr.Zero;
1258 }
1259 }
1260 }
1261 Body = IntPtr.Zero;
1262 }
1263 }
1264 else
1265 {
1266 _parent_scene.remActivePrim(this);
1267
1268 m_collisionCategories &= ~CollisionCategories.Body;
1269 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
1270
1271 if (prim_geom != IntPtr.Zero)
1272 {
1273 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1274 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1275 }
1276
1277
1278 Body = IntPtr.Zero;
1279 }
1280 }
1281 m_disabled = true;
1282 m_collisionscore = 0;
1283 }
1284
1285 private static Dictionary<IMesh, IntPtr> m_MeshToTriMeshMap = new Dictionary<IMesh, IntPtr>();
1286
1287 public void setMesh(OdeScene parent_scene, IMesh mesh)
1288 {
1289 // This sleeper is there to moderate how long it takes between
1290 // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object
1291
1292 //Thread.Sleep(10);
1293
1294 //Kill Body so that mesh can re-make the geom
1295 if (IsPhysical && Body != IntPtr.Zero)
1296 {
1297 if (childPrim)
1298 {
1299 if (_parent != null)
1300 {
1301 OdePrim parent = (OdePrim)_parent;
1302 parent.ChildDelink(this);
1303 }
1304 }
1305 else
1306 {
1307 disableBody();
1308 }
1309 }
1310
1311 IntPtr vertices, indices;
1312 int vertexCount, indexCount;
1313 int vertexStride, triStride;
1314 mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
1315 mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
1316
1317 mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
1318 if (m_MeshToTriMeshMap.ContainsKey(mesh))
1319 {
1320 _triMeshData = m_MeshToTriMeshMap[mesh];
1321 }
1322 else
1323 {
1324 _triMeshData = d.GeomTriMeshDataCreate();
1325
1326 d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
1327 d.GeomTriMeshDataPreprocess(_triMeshData);
1328 m_MeshToTriMeshMap[mesh] = _triMeshData;
1329 }
1330
1331 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1332 try
1333 {
1334 if (prim_geom == IntPtr.Zero)
1335 {
1336 SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
1337 }
1338 }
1339 catch (AccessViolationException)
1340 {
1341 m_log.Error("[PHYSICS]: MESH LOCKED");
1342 return;
1343 }
1344
1345
1346 // if (IsPhysical && Body == (IntPtr) 0)
1347 // {
1348 // Recreate the body
1349 // m_interpenetrationcount = 0;
1350 // m_collisionscore = 0;
1351
1352 // enableBody();
1353 // }
1354 }
1355
1356 public void ProcessTaints(float timestep) //=============================================================================
1357 {
1358 if (m_taintadd)
1359 {
1360 changeadd(timestep);
1361 }
1362
1363 if (prim_geom != IntPtr.Zero)
1364 {
1365 if (!_position.ApproxEquals(m_taintposition, 0f))
1366 changemove(timestep);
1367
1368 if (m_taintrot != _orientation)
1369 {
1370 if(childPrim && IsPhysical) // For physical child prim...
1371 {
1372 rotate(timestep);
1373 // KF: ODE will also rotate the parent prim!
1374 // so rotate the root back to where it was
1375 OdePrim parent = (OdePrim)_parent;
1376 parent.rotate(timestep);
1377 }
1378 else
1379 {
1380 //Just rotate the prim
1381 rotate(timestep);
1382 }
1383 }
1384 //
1385
1386 if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent))
1387 changePhysicsStatus(timestep);
1388 //
1389
1390 if (!_size.ApproxEquals(m_taintsize,0f))
1391 changesize(timestep);
1392 //
1393
1394 if (m_taintshape)
1395 changeshape(timestep);
1396 //
1397
1398 if (m_taintforce)
1399 changeAddForce(timestep);
1400
1401 if (m_taintaddangularforce)
1402 changeAddAngularForce(timestep);
1403
1404 if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f))
1405 changeSetTorque(timestep);
1406
1407 if (m_taintdisable)
1408 changedisable(timestep);
1409
1410 if (m_taintselected != m_isSelected)
1411 changeSelectedStatus(timestep);
1412
1413 if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f))
1414 changevelocity(timestep);
1415
1416 if (m_taintparent != _parent)
1417 changelink(timestep);
1418
1419 if (m_taintCollidesWater != m_collidesWater)
1420 changefloatonwater(timestep);
1421/* obsolete
1422 if (!m_angularLock.ApproxEquals(m_taintAngularLock,0f))
1423 changeAngularLock(timestep);
1424 */
1425 }
1426 else
1427 {
1428 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.)");
1429 }
1430 }
1431
1432/* obsolete
1433 private void changeAngularLock(float timestep)
1434 {
1435 if (_parent == null)
1436 {
1437 m_angularLock = m_taintAngularLock;
1438 m_angularLockSet = true;
1439 }
1440 }
1441 */
1442 private void changelink(float timestep)
1443 {
1444 // If the newly set parent is not null
1445 // create link
1446 if (_parent == null && m_taintparent != null)
1447 {
1448 if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim)
1449 {
1450 OdePrim obj = (OdePrim)m_taintparent;
1451 //obj.disableBody();
1452 obj.ParentPrim(this);
1453
1454 /*
1455 if (obj.Body != (IntPtr)0 && Body != (IntPtr)0 && obj.Body != Body)
1456 {
1457 _linkJointGroup = d.JointGroupCreate(0);
1458 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
1459 d.JointAttach(m_linkJoint, obj.Body, Body);
1460 d.JointSetFixed(m_linkJoint);
1461 }
1462 */
1463 }
1464 }
1465 // If the newly set parent is null
1466 // destroy link
1467 else if (_parent != null && m_taintparent == null)
1468 {
1469 if (_parent is OdePrim)
1470 {
1471 OdePrim obj = (OdePrim)_parent;
1472 obj.ChildDelink(this);
1473 childPrim = false;
1474 //_parent = null;
1475 }
1476
1477 /*
1478 if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0)
1479 d.JointGroupDestroy(_linkJointGroup);
1480
1481 _linkJointGroup = (IntPtr)0;
1482 m_linkJoint = (IntPtr)0;
1483 */
1484 }
1485
1486 _parent = m_taintparent;
1487 m_taintPhysics = m_isphysical;
1488 }
1489
1490 // I'm the parent
1491 // prim is the child
1492 public void ParentPrim(OdePrim prim)
1493 {
1494 if (this.m_localID != prim.m_localID)
1495 {
1496 if (Body == IntPtr.Zero)
1497 {
1498 Body = d.BodyCreate(_parent_scene.world);
1499 setMass();
1500 }
1501 if (Body != IntPtr.Zero)
1502 {
1503 lock (childrenPrim)
1504 {
1505 if (!childrenPrim.Contains(prim))
1506 {
1507 childrenPrim.Add(prim);
1508
1509 foreach (OdePrim prm in childrenPrim)
1510 {
1511 d.Mass m2;
1512 d.MassSetZero(out m2);
1513 d.MassSetBoxTotal(out m2, prim.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z);
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 d.MassRotate(ref m2, ref mat);
1525 d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z);
1526 d.MassAdd(ref pMass, ref m2);
1527 }
1528 foreach (OdePrim prm in childrenPrim)
1529 {
1530
1531 prm.m_collisionCategories |= CollisionCategories.Body;
1532 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1533
1534 if (prm.prim_geom == IntPtr.Zero)
1535 {
1536 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet");
1537 continue;
1538 }
1539//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + m_primName);
1540 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
1541 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
1542
1543
1544 d.Quaternion quat = new d.Quaternion();
1545 quat.W = prm._orientation.W;
1546 quat.X = prm._orientation.X;
1547 quat.Y = prm._orientation.Y;
1548 quat.Z = prm._orientation.Z;
1549
1550 d.Matrix3 mat = new d.Matrix3();
1551 d.RfromQ(out mat, ref quat);
1552 if (Body != IntPtr.Zero)
1553 {
1554 d.GeomSetBody(prm.prim_geom, Body);
1555 prm.childPrim = true;
1556 d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z);
1557 //d.GeomSetOffsetPosition(prim.prim_geom,
1558 // (Position.X - prm.Position.X) - pMass.c.X,
1559 // (Position.Y - prm.Position.Y) - pMass.c.Y,
1560 // (Position.Z - prm.Position.Z) - pMass.c.Z);
1561 d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat);
1562 //d.GeomSetOffsetRotation(prm.prim_geom, ref mat);
1563 d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
1564 d.BodySetMass(Body, ref pMass);
1565 }
1566 else
1567 {
1568 m_log.Debug("[PHYSICS]:I ain't got no boooooooooddy, no body");
1569 }
1570
1571
1572 prm.m_interpenetrationcount = 0;
1573 prm.m_collisionscore = 0;
1574 prm.m_disabled = false;
1575
1576 prm.Body = Body;
1577 _parent_scene.addActivePrim(prm);
1578 }
1579 m_collisionCategories |= CollisionCategories.Body;
1580 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1581
1582//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + m_primName);
1583 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1584//Console.WriteLine(" Post GeomSetCategoryBits 2");
1585 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1586
1587
1588 d.Quaternion quat2 = new d.Quaternion();
1589 quat2.W = _orientation.W;
1590 quat2.X = _orientation.X;
1591 quat2.Y = _orientation.Y;
1592 quat2.Z = _orientation.Z;
1593
1594 d.Matrix3 mat2 = new d.Matrix3();
1595 d.RfromQ(out mat2, ref quat2);
1596 d.GeomSetBody(prim_geom, Body);
1597 d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z);
1598 //d.GeomSetOffsetPosition(prim.prim_geom,
1599 // (Position.X - prm.Position.X) - pMass.c.X,
1600 // (Position.Y - prm.Position.Y) - pMass.c.Y,
1601 // (Position.Z - prm.Position.Z) - pMass.c.Z);
1602 //d.GeomSetOffsetRotation(prim_geom, ref mat2);
1603 d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
1604 d.BodySetMass(Body, ref pMass);
1605
1606 d.BodySetAutoDisableFlag(Body, true);
1607 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
1608
1609
1610 m_interpenetrationcount = 0;
1611 m_collisionscore = 0;
1612 m_disabled = false;
1613
1614 d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
1615 if (m_type != Vehicle.TYPE_NONE) Enable(Body, _parent_scene);
1616 _parent_scene.addActivePrim(this);
1617 }
1618 }
1619 }
1620 }
1621
1622 }
1623
1624 private void ChildSetGeom(OdePrim odePrim)
1625 {
1626 //if (m_isphysical && Body != IntPtr.Zero)
1627 lock (childrenPrim)
1628 {
1629 foreach (OdePrim prm in childrenPrim)
1630 {
1631 //prm.childPrim = true;
1632 prm.disableBody();
1633 //prm.m_taintparent = null;
1634 //prm._parent = null;
1635 //prm.m_taintPhysics = false;
1636 //prm.m_disabled = true;
1637 //prm.childPrim = false;
1638 }
1639 }
1640 disableBody();
1641
1642
1643 if (Body != IntPtr.Zero)
1644 {
1645 _parent_scene.remActivePrim(this);
1646 }
1647
1648 lock (childrenPrim)
1649 {
1650 foreach (OdePrim prm in childrenPrim)
1651 {
1652 ParentPrim(prm);
1653 }
1654 }
1655
1656 }
1657
1658 private void ChildDelink(OdePrim odePrim)
1659 {
1660 // Okay, we have a delinked child.. need to rebuild the body.
1661 lock (childrenPrim)
1662 {
1663 foreach (OdePrim prm in childrenPrim)
1664 {
1665 prm.childPrim = true;
1666 prm.disableBody();
1667 //prm.m_taintparent = null;
1668 //prm._parent = null;
1669 //prm.m_taintPhysics = false;
1670 //prm.m_disabled = true;
1671 //prm.childPrim = false;
1672 }
1673 }
1674 disableBody();
1675
1676 lock (childrenPrim)
1677 {
1678 childrenPrim.Remove(odePrim);
1679 }
1680
1681 if (Body != IntPtr.Zero)
1682 {
1683 _parent_scene.remActivePrim(this);
1684 }
1685
1686 lock (childrenPrim)
1687 {
1688 foreach (OdePrim prm in childrenPrim)
1689 {
1690 ParentPrim(prm);
1691 }
1692 }
1693 }
1694
1695 private void changeSelectedStatus(float timestep)
1696 {
1697 if (m_taintselected)
1698 {
1699 m_collisionCategories = CollisionCategories.Selected;
1700 m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
1701
1702 // We do the body disable soft twice because 'in theory' a collision could have happened
1703 // in between the disabling and the collision properties setting
1704 // which would wake the physical body up from a soft disabling and potentially cause it to fall
1705 // through the ground.
1706
1707 // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select
1708 // just one part of the assembly, the rest of the assembly is non-selected and still simulating,
1709 // so that causes the selected part to wake up and continue moving.
1710
1711 // even if you select all parts of a jointed assembly, it is not guaranteed that the entire
1712 // assembly will stop simulating during the selection, because of the lack of atomicity
1713 // of select operations (their processing could be interrupted by a thread switch, causing
1714 // simulation to continue before all of the selected object notifications trickle down to
1715 // the physics engine).
1716
1717 // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are
1718 // selected and disabled. then, due to a thread switch, the selection processing is
1719 // interrupted and the physics engine continues to simulate, so the last 50 items, whose
1720 // selection was not yet processed, continues to simulate. this wakes up ALL of the
1721 // first 50 again. then the last 50 are disabled. then the first 50, which were just woken
1722 // up, start simulating again, which in turn wakes up the last 50.
1723
1724 if (m_isphysical)
1725 {
1726 disableBodySoft();
1727 }
1728
1729 if (prim_geom != IntPtr.Zero)
1730 {
1731 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1732 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1733 }
1734
1735 if (m_isphysical)
1736 {
1737 disableBodySoft();
1738 }
1739 }
1740 else
1741 {
1742 m_collisionCategories = CollisionCategories.Geom;
1743
1744 if (m_isphysical)
1745 m_collisionCategories |= CollisionCategories.Body;
1746
1747 m_collisionFlags = m_default_collisionFlags;
1748
1749 if (m_collidesLand)
1750 m_collisionFlags |= CollisionCategories.Land;
1751 if (m_collidesWater)
1752 m_collisionFlags |= CollisionCategories.Water;
1753
1754 if (prim_geom != IntPtr.Zero)
1755 {
1756 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1757 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1758 }
1759 if (m_isphysical)
1760 {
1761 if (Body != IntPtr.Zero)
1762 {
1763 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1764 d.BodySetForce(Body, 0, 0, 0);
1765 enableBodySoft();
1766 }
1767 }
1768 }
1769
1770 resetCollisionAccounting();
1771 m_isSelected = m_taintselected;
1772 }//end changeSelectedStatus
1773
1774 public void ResetTaints()
1775 {
1776 m_taintposition = _position;
1777 m_taintrot = _orientation;
1778 m_taintPhysics = m_isphysical;
1779 m_taintselected = m_isSelected;
1780 m_taintsize = _size;
1781 m_taintshape = false;
1782 m_taintforce = false;
1783 m_taintdisable = false;
1784 m_taintVelocity = Vector3.Zero;
1785 }
1786
1787 public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh)
1788 {
1789//Console.WriteLine("CreateGeom:");
1790 if (_mesh != null) // Special - make mesh
1791 {
1792 setMesh(_parent_scene, _mesh);
1793 }
1794 else // not a mesh
1795 {
1796 if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1) // special profile??
1797 {
1798 if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z) // Equi-size
1799 {
1800 if (((_size.X / 2f) > 0f)) // Has size
1801 {
1802 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1803 try
1804 {
1805//Console.WriteLine(" CreateGeom 1");
1806 SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2));
1807 }
1808 catch (AccessViolationException)
1809 {
1810 m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
1811 ode.dunlock(_parent_scene.world);
1812 return;
1813 }
1814 }
1815 else
1816 {
1817 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1818 try
1819 {
1820//Console.WriteLine(" CreateGeom 2");
1821 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1822 }
1823 catch (AccessViolationException)
1824 {
1825 m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
1826 ode.dunlock(_parent_scene.world);
1827 return;
1828 }
1829 }
1830 }
1831 else // not equi-size
1832 {
1833 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1834 try
1835 {
1836//Console.WriteLine(" CreateGeom 3");
1837 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1838 }
1839 catch (AccessViolationException)
1840 {
1841 m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
1842 ode.dunlock(_parent_scene.world);
1843 return;
1844 }
1845 }
1846 }
1847
1848 else // not special profile
1849 {
1850 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1851 try
1852 {
1853//Console.WriteLine(" CreateGeom 4");
1854 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1855 }
1856 catch (AccessViolationException)
1857 {
1858 m_log.Warn("[PHYSICS]: Unable to create physics proxy for object");
1859 ode.dunlock(_parent_scene.world);
1860 return;
1861 }
1862 }
1863 }
1864 }
1865
1866 public void changeadd(float timestep)
1867 {
1868 int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
1869 IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
1870
1871 if (targetspace == IntPtr.Zero)
1872 targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
1873
1874 m_targetSpace = targetspace;
1875
1876 if (_mesh == null)
1877 {
1878 if (_parent_scene.needsMeshing(_pbs))
1879 {
1880 // Don't need to re-enable body.. it's done in SetMesh
1881 _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
1882 // createmesh returns null when it's a shape that isn't a cube.
1883 // m_log.Debug(m_localID);
1884 }
1885 }
1886
1887
1888 lock (_parent_scene.OdeLock)
1889 {
1890//Console.WriteLine("changeadd 1");
1891 CreateGeom(m_targetSpace, _mesh);
1892
1893 if (prim_geom != IntPtr.Zero)
1894 {
1895 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
1896 d.Quaternion myrot = new d.Quaternion();
1897 myrot.X = _orientation.X;
1898 myrot.Y = _orientation.Y;
1899 myrot.Z = _orientation.Z;
1900 myrot.W = _orientation.W;
1901 d.GeomSetQuaternion(prim_geom, ref myrot);
1902 }
1903
1904 if (m_isphysical && Body == IntPtr.Zero)
1905 {
1906 enableBody();
1907 }
1908 }
1909
1910 changeSelectedStatus(timestep);
1911
1912 m_taintadd = false;
1913 }
1914
1915 public void changemove(float timestep)
1916 {
1917//Console.WriteLine("changemove sing/root {0} to {1}", m_primName, _position );
1918 if (m_isphysical)
1919 {
1920//Console.WriteLine("phys {0} {1} {2}", m_disabled, m_taintremove, childPrim);
1921// if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits!
1922 if (!m_taintremove && !childPrim)
1923 {
1924 if (Body == IntPtr.Zero)
1925 enableBody();
1926 //Prim auto disable after 20 frames,
1927 //if you move it, re-enable the prim manually.
1928 if (_parent != null)
1929 {
1930 if (m_linkJoint != IntPtr.Zero)
1931 {
1932 d.JointDestroy(m_linkJoint);
1933 m_linkJoint = IntPtr.Zero;
1934 }
1935 }
1936 if (Body != IntPtr.Zero)
1937 {
1938 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
1939
1940 if (_parent != null)
1941 {
1942 OdePrim odParent = (OdePrim)_parent;
1943 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
1944 {
1945// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
1946Console.WriteLine(" JointCreateFixed");
1947 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
1948 d.JointAttach(m_linkJoint, Body, odParent.Body);
1949 d.JointSetFixed(m_linkJoint);
1950 }
1951 }
1952 d.BodyEnable(Body);
1953 if (m_type != Vehicle.TYPE_NONE)
1954 {
1955 Enable(Body, _parent_scene);
1956 }
1957 }
1958 else
1959 {
1960 m_log.Warn("[PHYSICS]: Body Still null after enableBody(). This is a crash scenario.");
1961 }
1962 }
1963 //else
1964 // {
1965 //m_log.Debug("[BUG]: race!");
1966 //}
1967 }
1968 else
1969 {
1970 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
1971 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
1972 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1973
1974 IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace);
1975 m_targetSpace = tempspace;
1976
1977 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1978 if (prim_geom != IntPtr.Zero)
1979 {
1980 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
1981
1982 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1983 d.SpaceAdd(m_targetSpace, prim_geom);
1984 }
1985 }
1986
1987 changeSelectedStatus(timestep);
1988
1989 resetCollisionAccounting();
1990 m_taintposition = _position;
1991 }
1992
1993
1994
1995 public void rotate(float timestep)
1996 {
1997 d.Quaternion myrot = new d.Quaternion();
1998 myrot.X = _orientation.X;
1999 myrot.Y = _orientation.Y;
2000 myrot.Z = _orientation.Z;
2001 myrot.W = _orientation.W;
2002 if (Body != IntPtr.Zero)
2003 {
2004 // KF: If this is a root prim do BodySet
2005 d.BodySetQuaternion(Body, ref myrot);
2006 }
2007 else
2008 {
2009 // daughter prim, do Geom set
2010 d.GeomSetQuaternion(prim_geom, ref myrot);
2011 }
2012
2013 resetCollisionAccounting();
2014 m_taintrot = _orientation;
2015 }
2016
2017 private void resetCollisionAccounting()
2018 {
2019 m_collisionscore = 0;
2020 m_interpenetrationcount = 0;
2021 m_disabled = false;
2022 }
2023
2024 public void changedisable(float timestep)
2025 {
2026 m_disabled = true;
2027 if (Body != IntPtr.Zero)
2028 {
2029 d.BodyDisable(Body);
2030 Body = IntPtr.Zero;
2031 }
2032
2033 m_taintdisable = false;
2034 }
2035
2036 public void changePhysicsStatus(float timestep)
2037 {
2038 if (m_isphysical == true)
2039 {
2040 if (Body == IntPtr.Zero)
2041 {
2042 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
2043 {
2044 changeshape(2f);
2045 }
2046 else
2047 {
2048 enableBody();
2049 }
2050 }
2051 }
2052 else
2053 {
2054 if (Body != IntPtr.Zero)
2055 {
2056 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
2057 {
2058 _mesh = null;
2059//Console.WriteLine("changePhysicsStatus for " + m_primName );
2060 changeadd(2f);
2061 }
2062 if (childPrim)
2063 {
2064 if (_parent != null)
2065 {
2066 OdePrim parent = (OdePrim)_parent;
2067 parent.ChildDelink(this);
2068 }
2069 }
2070 else
2071 {
2072 disableBody();
2073 }
2074 }
2075 }
2076
2077 changeSelectedStatus(timestep);
2078
2079 resetCollisionAccounting();
2080 m_taintPhysics = m_isphysical;
2081 }
2082
2083 public void changesize(float timestamp)
2084 {
2085
2086 string oldname = _parent_scene.geom_name_map[prim_geom];
2087
2088 if (_size.X <= 0) _size.X = 0.01f;
2089 if (_size.Y <= 0) _size.Y = 0.01f;
2090 if (_size.Z <= 0) _size.Z = 0.01f;
2091
2092 // Cleanup of old prim geometry
2093 if (_mesh != null)
2094 {
2095 // Cleanup meshing here
2096 }
2097 //kill body to rebuild
2098 if (IsPhysical && Body != IntPtr.Zero)
2099 {
2100 if (childPrim)
2101 {
2102 if (_parent != null)
2103 {
2104 OdePrim parent = (OdePrim)_parent;
2105 parent.ChildDelink(this);
2106 }
2107 }
2108 else
2109 {
2110 disableBody();
2111 }
2112 }
2113 if (d.SpaceQuery(m_targetSpace, prim_geom))
2114 {
2115 _parent_scene.waitForSpaceUnlock(m_targetSpace);
2116 d.SpaceRemove(m_targetSpace, prim_geom);
2117 }
2118 // we don't need to do space calculation because the client sends a position update also.
2119
2120 // Construction of new prim
2121 if (_parent_scene.needsMeshing(_pbs))
2122 {
2123 float meshlod = _parent_scene.meshSculptLOD;
2124
2125 if (IsPhysical)
2126 meshlod = _parent_scene.MeshSculptphysicalLOD;
2127 // Don't need to re-enable body.. it's done in SetMesh
2128
2129 IMesh mesh = null;
2130
2131 if (_parent_scene.needsMeshing(_pbs))
2132 mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
2133
2134 //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
2135//Console.WriteLine("changesize 1");
2136 CreateGeom(m_targetSpace, mesh);
2137
2138
2139 }
2140 else
2141 {
2142 _mesh = null;
2143//Console.WriteLine("changesize 2");
2144 CreateGeom(m_targetSpace, _mesh);
2145 }
2146
2147 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2148 d.Quaternion myrot = new d.Quaternion();
2149 myrot.X = _orientation.X;
2150 myrot.Y = _orientation.Y;
2151 myrot.Z = _orientation.Z;
2152 myrot.W = _orientation.W;
2153 d.GeomSetQuaternion(prim_geom, ref myrot);
2154
2155 //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
2156 if (IsPhysical && Body == IntPtr.Zero && !childPrim)
2157 {
2158 // Re creates body on size.
2159 // EnableBody also does setMass()
2160 enableBody();
2161 d.BodyEnable(Body);
2162 }
2163
2164 _parent_scene.geom_name_map[prim_geom] = oldname;
2165
2166 changeSelectedStatus(timestamp);
2167 if (childPrim)
2168 {
2169 if (_parent is OdePrim)
2170 {
2171 OdePrim parent = (OdePrim)_parent;
2172 parent.ChildSetGeom(this);
2173 }
2174 }
2175 resetCollisionAccounting();
2176 m_taintsize = _size;
2177 }
2178
2179
2180
2181 public void changefloatonwater(float timestep)
2182 {
2183 m_collidesWater = m_taintCollidesWater;
2184
2185 if (prim_geom != IntPtr.Zero)
2186 {
2187 if (m_collidesWater)
2188 {
2189 m_collisionFlags |= CollisionCategories.Water;
2190 }
2191 else
2192 {
2193 m_collisionFlags &= ~CollisionCategories.Water;
2194 }
2195 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2196 }
2197 }
2198
2199 public void changeshape(float timestamp)
2200 {
2201 string oldname = _parent_scene.geom_name_map[prim_geom];
2202
2203 // Cleanup of old prim geometry and Bodies
2204 if (IsPhysical && Body != IntPtr.Zero)
2205 {
2206 if (childPrim)
2207 {
2208 if (_parent != null)
2209 {
2210 OdePrim parent = (OdePrim)_parent;
2211 parent.ChildDelink(this);
2212 }
2213 }
2214 else
2215 {
2216 disableBody();
2217 }
2218 }
2219
2220
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 > 50)
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
3206 // Rotation lock =====================================
3207 if(m_rotateEnableUpdate)
3208 {
3209 // Snapshot current angles, set up Amotor(s)
3210 m_rotateEnableUpdate = false;
3211 m_rotateEnable = m_rotateEnableRequest;
3212Console.WriteLine("RotEnable {0} = {1}",m_primName, m_rotateEnable);
3213
3214 if (Amotor != IntPtr.Zero)
3215 {
3216 d.JointDestroy(Amotor);
3217 Amotor = IntPtr.Zero;
3218Console.WriteLine("Old Amotor Destroyed");
3219 }
3220
3221 if (!m_rotateEnable.ApproxEquals(Vector3.One, 0.003f))
3222 { // not all are enabled
3223 d.Quaternion r = d.BodyGetQuaternion(Body);
3224 Quaternion locrot = new Quaternion(r.X, r.Y, r.Z, r.W);
3225 // extract the axes vectors
3226 Vector3 vX = new Vector3(1f,0f,0f);
3227 Vector3 vY = new Vector3(0f,1f,0f);
3228 Vector3 vZ = new Vector3(0f,0f,1f);
3229 vX = vX * locrot;
3230 vY = vY * locrot;
3231 vZ = vZ * locrot;
3232 // snapshot the current angle vectors
3233 m_lockX = vX;
3234 m_lockY = vY;
3235 m_lockZ = vZ;
3236 // m_lockRot = locrot;
3237 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
3238 d.JointAttach(Amotor, Body, IntPtr.Zero);
3239 d.JointSetAMotorMode(Amotor, 0); // User mode??
3240Console.WriteLine("New Amotor Created for {0}", m_primName);
3241
3242 float axisnum = 3; // how many to lock
3243 axisnum = (axisnum - (m_rotateEnable.X + m_rotateEnable.Y + m_rotateEnable.Z));
3244 d.JointSetAMotorNumAxes(Amotor,(int)axisnum);
3245Console.WriteLine("AxisNum={0}",(int)axisnum);
3246
3247 int i = 0;
3248
3249 if (m_rotateEnable.X == 0)
3250 {
3251 d.JointSetAMotorAxis(Amotor, i, 0, m_lockX.X, m_lockX.Y, m_lockX.Z);
3252Console.WriteLine("AxisX {0} set to {1}", i, m_lockX);
3253 i++;
3254 }
3255
3256 if (m_rotateEnable.Y == 0)
3257 {
3258 d.JointSetAMotorAxis(Amotor, i, 0, m_lockY.X, m_lockY.Y, m_lockY.Z);
3259Console.WriteLine("AxisY {0} set to {1}", i, m_lockY);
3260 i++;
3261 }
3262
3263 if (m_rotateEnable.Z == 0)
3264 {
3265 d.JointSetAMotorAxis(Amotor, i, 0, m_lockZ.X, m_lockZ.Y, m_lockZ.Z);
3266Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ);
3267 i++;
3268 }
3269
3270 // These lowstops and high stops are effectively (no wiggle room)
3271 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, 0f);
3272 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, 0f);
3273 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, 0f);
3274 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
3275 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
3276 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
3277 d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 0f);
3278 d.JointSetAMotorParam(Amotor, (int) dParam.Vel3, 0f);
3279 d.JointSetAMotorParam(Amotor, (int) dParam.Vel2, 0f);
3280 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f);
3281 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f);
3282 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f);
3283 } // else none are locked
3284 } // end Rotation Update
3285
3286
3287 // VEHICLE processing ==========================================
3288 if (m_type != Vehicle.TYPE_NONE)
3289 {
3290 // get body attitude
3291 d.Quaternion rot = d.BodyGetQuaternion(Body);
3292 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
3293 Quaternion irotq = Quaternion.Inverse(rotq);
3294
3295 // VEHICLE Linear Motion
3296 d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame
3297 Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z);
3298 m_lLinObjectVel = vel_now * irotq;
3299
3300 if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate
3301 {
3302 if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f)
3303 {
3304 float decayfactor = m_linearMotorDecayTimescale/timestep;
3305 Vector3 decayAmount = (m_lLinMotorDVel/decayfactor);
3306 m_lLinMotorDVel -= decayAmount;
3307 }
3308 else
3309 {
3310 float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale)));
3311 Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep;
3312 m_lLinMotorDVel -= decel;
3313 }
3314 if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
3315 {
3316 m_lLinMotorDVel = Vector3.Zero;
3317 }
3318
3319 /* else
3320 {
3321 if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X;
3322 if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y;
3323 if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z;
3324 } */
3325 } // end linear motor decay
3326
3327 if ( (! m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
3328 {
3329 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
3330 if (m_linearMotorTimescale < 300.0f)
3331 {
3332 Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel;
3333 float linfactor = m_linearMotorTimescale/timestep;
3334 Vector3 attackAmount = (attack_error/linfactor) * 1.3f;
3335 m_lLinObjectVel += attackAmount;
3336 }
3337 if (m_linearFrictionTimescale.X < 300.0f)
3338 {
3339 float fricfactor = m_linearFrictionTimescale.X / timestep;
3340 float fricX = m_lLinObjectVel.X / fricfactor;
3341 m_lLinObjectVel.X -= fricX;
3342 }
3343 if (m_linearFrictionTimescale.Y < 300.0f)
3344 {
3345 float fricfactor = m_linearFrictionTimescale.Y / timestep;
3346 float fricY = m_lLinObjectVel.Y / fricfactor;
3347 m_lLinObjectVel.Y -= fricY;
3348 }
3349 if (m_linearFrictionTimescale.Z < 300.0f)
3350 {
3351 float fricfactor = m_linearFrictionTimescale.Z / timestep;
3352 float fricZ = m_lLinObjectVel.Z / fricfactor;
3353 m_lLinObjectVel.Z -= fricZ;
3354 }
3355 }
3356 m_wLinObjectVel = m_lLinObjectVel * rotq;
3357
3358 // Gravity and Buoyancy
3359 Vector3 grav = Vector3.Zero;
3360 if(m_VehicleBuoyancy < 1.0f)
3361 {
3362 // There is some gravity, make a gravity force vector
3363 // that is applied after object velocity.
3364 d.Mass objMass;
3365 d.BodyGetMass(Body, out objMass);
3366 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
3367 grav.Z = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force
3368 } // else its 1.0, no gravity.
3369
3370 // Hovering
3371 if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
3372 {
3373 // We should hover, get the target height
3374 d.Vector3 pos = d.BodyGetPosition(Body);
3375 if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
3376 {
3377 m_VhoverTargetHeight = _parent_scene.GetWaterLevel() + m_VhoverHeight;
3378 }
3379 else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
3380 {
3381 m_VhoverTargetHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
3382 }
3383 else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
3384 {
3385 m_VhoverTargetHeight = m_VhoverHeight;
3386 }
3387
3388 if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
3389 {
3390 // If body is aready heigher, use its height as target height
3391 if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
3392 }
3393
3394// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
3395// m_VhoverTimescale = 0f; // time to acheive height
3396// timestep is time since last frame,in secs
3397 float herr0 = pos.Z - m_VhoverTargetHeight;
3398 // Replace Vertical speed with correction figure if significant
3399 if(Math.Abs(herr0) > 0.01f )
3400 {
3401 //? d.Mass objMass;
3402 //? d.BodyGetMass(Body, out objMass);
3403 m_wLinObjectVel.Z = - ( (herr0 * timestep * 50.0f) / m_VhoverTimescale);
3404 //KF: m_VhoverEfficiency is not yet implemented
3405 }
3406 else
3407 {
3408 m_wLinObjectVel.Z = 0f;
3409 }
3410 }
3411 else
3412 { // not hovering
3413 if (m_wLinObjectVel.Z == 0f)
3414 { // Gravity rules
3415 m_wLinObjectVel.Z = vel_now.Z;
3416 } // else the motor has it
3417 }
3418 linvel = m_wLinObjectVel;
3419
3420 // Vehicle Linear Motion done =======================================
3421 // Apply velocity
3422 d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z);
3423 // apply gravity force
3424 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
3425//if(frcount == 0) Console.WriteLine("Grav {0}", grav);
3426 // end MoveLinear()
3427
3428
3429 // MoveAngular
3430 /*
3431 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
3432
3433 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
3434 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
3435 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
3436
3437 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
3438 private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body
3439 */
3440//if(frcount == 0) Console.WriteLine("MoveAngular ");
3441
3442 d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body);
3443 Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z);
3444 angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation
3445
3446//if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel);
3447
3448 // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack.
3449 float atk_decayfactor = 23.0f / (m_angularMotorTimescale * timestep);
3450 m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor;
3451 // Decay Angular Motor 2.
3452 if (m_angularMotorDecayTimescale < 300.0f)
3453 {
3454 if ( Vector3.Mag(m_angularMotorDVel) < 1.0f)
3455 {
3456 float decayfactor = (m_angularMotorDecayTimescale)/timestep;
3457 Vector3 decayAmount = (m_angularMotorDVel/decayfactor);
3458 m_angularMotorDVel -= decayAmount;
3459 }
3460 else
3461 {
3462 Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale;
3463 m_angularMotorDVel -= decel;
3464 }
3465
3466 if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
3467 {
3468 m_angularMotorDVel = Vector3.Zero;
3469 }
3470 else
3471 {
3472 if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X;
3473 if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y;
3474 if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z;
3475 }
3476 } // end decay angular motor
3477//if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel);
3478
3479//if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel);
3480
3481 if ( (! m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
3482 { // if motor or object have motion
3483 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
3484
3485 if (m_angularMotorTimescale < 300.0f)
3486 {
3487 Vector3 attack_error = m_angularMotorDVel - angObjectVel;
3488 float angfactor = m_angularMotorTimescale/timestep;
3489 Vector3 attackAmount = (attack_error/angfactor);
3490 angObjectVel += attackAmount;
3491//if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount);
3492//if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel);
3493 }
3494
3495 angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep);
3496 angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep);
3497 angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep);
3498 } // else no signif. motion
3499
3500//if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel);
3501 // Bank section tba
3502 // Deflection section tba
3503//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel);
3504
3505
3506 /* // Rotation Axis Disables:
3507 if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f))
3508 {
3509 if (m_angularEnable.X == 0)
3510 angObjectVel.X = 0f;
3511 if (m_angularEnable.Y == 0)
3512 angObjectVel.Y = 0f;
3513 if (m_angularEnable.Z == 0)
3514 angObjectVel.Z = 0f;
3515 }
3516 */
3517 angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation
3518
3519 // Vertical attractor section
3520 Vector3 vertattr = Vector3.Zero;
3521
3522 if(m_verticalAttractionTimescale < 300)
3523 {
3524 float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep);
3525 // make a vector pointing up
3526 Vector3 verterr = Vector3.Zero;
3527 verterr.Z = 1.0f;
3528 // rotate it to Body Angle
3529 verterr = verterr * rotq;
3530 // 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.
3531 // 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
3532 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
3533
3534 if (verterr.Z < 0.0f)
3535 { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to
3536 // vertical, BUT for some reason a z-rotation is imparted to the object. TBI.
3537//Console.WriteLine("InvertFlip");
3538 verterr.X = 2.0f - verterr.X;
3539 verterr.Y = 2.0f - verterr.Y;
3540 }
3541 verterr *= 0.5f;
3542 // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt)
3543 Vector3 xyav = angObjectVel;
3544 xyav.Z = 0.0f;
3545 if ((!xyav.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f))
3546 {
3547 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
3548 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
3549 vertattr.X = verterr.Y;
3550 vertattr.Y = - verterr.X;
3551 vertattr.Z = 0f;
3552//if(frcount == 0) Console.WriteLine("VAerr=" + verterr);
3553
3554 // scaling appears better usingsquare-law
3555 float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
3556 float bounce = 1.0f - damped;
3557 // 0 = crit damp, 1 = bouncy
3558 float oavz = angObjectVel.Z; // retain z velocity
3559 // time-scaled correction, which sums, therefore is bouncy:
3560 angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce;
3561 // damped, good @ < 90:
3562 angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped);
3563 angObjectVel.Z = oavz;
3564//if(frcount == 0) Console.WriteLine("VA+");
3565//Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel);
3566 }
3567 else
3568 {
3569 // else error is very small
3570 angObjectVel.X = 0f;
3571 angObjectVel.Y = 0f;
3572//if(frcount == 0) Console.WriteLine("VA0");
3573 }
3574 } // else vertical attractor is off
3575//if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel);
3576
3577
3578 m_lastAngularVelocity = angObjectVel;
3579 // apply Angular Velocity to body
3580 d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
3581//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity);
3582
3583 } // end VEHICLES
3584 else
3585 {
3586 // Dyamics (NON-'VEHICLES') are dealt with here ================================================================
3587
3588 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
3589
3590 /// Dynamics Buoyancy
3591 //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle.
3592 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
3593 // NB Prims in ODE are no subject to global gravity
3594 // This should only affect gravity operations
3595
3596 float m_mass = CalculateMass();
3597 // calculate z-force due togravity on object.
3598 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass
3599
3600 if ((m_usePID) && (m_PIDTau > 0.0f)) // Dynamics llMoveToTarget.
3601 {
3602 fz = 0; // llMoveToTarget ignores gravity.
3603 // it also ignores mass of object, and any physical resting on it.
3604 // Vector3 m_PIDTarget is where we are going
3605 // float m_PIDTau is time to get there
3606 fx = 0;
3607 fy = 0;
3608 d.Vector3 pos = d.BodyGetPosition(Body);
3609 Vector3 error = new Vector3(
3610 (m_PIDTarget.X - pos.X),
3611 (m_PIDTarget.Y - pos.Y),
3612 (m_PIDTarget.Z - pos.Z));
3613 if (error.ApproxEquals(Vector3.Zero,0.01f))
3614 { // Very close, Jump there and quit move
3615 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3616 _target_velocity = Vector3.Zero;
3617 d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
3618 }
3619 else
3620 {
3621 float scale = 50.0f * timestep / m_PIDTau;
3622 if ((error.ApproxEquals(Vector3.Zero,0.5f)) && (_target_velocity != Vector3.Zero))
3623 {
3624 // Nearby, quit update of velocity
3625 }
3626 else
3627 { // Far, calc damped velocity
3628 _target_velocity = error * scale;
3629 }
3630 d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
3631 }
3632 } // end PID MoveToTarget
3633
3634 /* Original OS implementation: Does not work correctly as another phys object resting on THIS object purturbs its position.
3635 This is incorrect behavior. llMoveToTarget must move the Body no matter what phys object is resting on it.
3636
3637 //if (!d.BodyIsEnabled(Body))
3638 //d.BodySetForce(Body, 0f, 0f, 0f);
3639
3640 // no lock; for now it's only called from within Simulate()
3641
3642 // If the PID Controller isn't active then we set our force
3643 // calculating base velocity to the current position
3644
3645 if ((m_PIDTau < 1) && (m_PIDTau != 0))
3646 {
3647 //PID_G = PID_G / m_PIDTau;
3648 m_PIDTau = 1;
3649 }
3650
3651 if ((PID_G - m_PIDTau) <= 0)
3652 {
3653 PID_G = m_PIDTau + 1;
3654 }
3655 //PidStatus = true;
3656
3657 // PhysicsVector vec = new PhysicsVector();
3658// d.Vector3 vel = d.BodyGetLinearVel(Body);
3659
3660 d.Vector3 pos = d.BodyGetPosition(Body);
3661 _target_velocity =
3662 new Vector3(
3663 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
3664 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
3665 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
3666 );
3667
3668if(frcount == 0) Console.WriteLine("PID {0} b={1} fz={2} vel={3}", m_primName, m_buoyancy, fz, _target_velocity);
3669 // if velocity is zero, use position control; otherwise, velocity control
3670
3671 if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
3672 {
3673 // keep track of where we stopped. No more slippin' & slidin'
3674
3675 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3676 // react to the physics scene by moving it's position.
3677 // Avatar to Avatar collisions
3678 // Prim to avatar collisions
3679
3680 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
3681 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
3682 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
3683 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3684 d.BodySetLinearVel(Body, 0, 0, 0);
3685 d.BodyAddForce(Body, 0, 0, fz);
3686 // return;
3687 }
3688 else
3689 {
3690 _zeroFlag = false;
3691
3692 // We're flying and colliding with something
3693 fx = ((_target_velocity.X) - vel.X) * (PID_D);
3694 fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
3695
3696 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
3697
3698 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
3699 }
3700 } // end if (m_usePID)
3701 End of old PID system */
3702
3703
3704 /// Dynamics Hover ===================================================================================
3705 // Hover PID Controller can only run if the PIDcontroller is not in use.
3706 if (m_useHoverPID && !m_usePID)
3707 {
3708//Console.WriteLine("Hover " + m_primName);
3709
3710 // If we're using the PID controller, then we have no gravity
3711 fz = (-1 * _parent_scene.gravityz) * m_mass;
3712
3713 // no lock; for now it's only called from within Simulate()
3714
3715 // If the PID Controller isn't active then we set our force
3716 // calculating base velocity to the current position
3717
3718 if ((m_PIDTau < 1))
3719 {
3720 PID_G = PID_G / m_PIDTau;
3721 }
3722
3723 if ((PID_G - m_PIDTau) <= 0)
3724 {
3725 PID_G = m_PIDTau + 1;
3726 }
3727
3728
3729 // Where are we, and where are we headed?
3730 d.Vector3 pos = d.BodyGetPosition(Body);
3731// d.Vector3 vel = d.BodyGetLinearVel(Body);
3732
3733
3734 // Non-Vehicles have a limited set of Hover options.
3735 // determine what our target height really is based on HoverType
3736 switch (m_PIDHoverType)
3737 {
3738 case PIDHoverType.Ground:
3739 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3740 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3741 break;
3742 case PIDHoverType.GroundAndWater:
3743 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3744 m_waterHeight = _parent_scene.GetWaterLevel();
3745 if (m_groundHeight > m_waterHeight)
3746 {
3747 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3748 }
3749 else
3750 {
3751 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
3752 }
3753 break;
3754
3755 } // end switch (m_PIDHoverType)
3756
3757
3758 _target_velocity =
3759 new Vector3(0.0f, 0.0f,
3760 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
3761 );
3762
3763 // if velocity is zero, use position control; otherwise, velocity control
3764
3765 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
3766 {
3767 // keep track of where we stopped. No more slippin' & slidin'
3768
3769 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3770 // react to the physics scene by moving it's position.
3771 // Avatar to Avatar collisions
3772 // Prim to avatar collisions
3773 d.Vector3 dlinvel = vel;
3774
3775 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
3776 d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z);
3777 d.BodyAddForce(Body, 0, 0, fz);
3778 //KF this prevents furthur motions return;
3779 }
3780 else
3781 {
3782 _zeroFlag = false;
3783
3784 // We're flying and colliding with something
3785 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
3786 }
3787 } // end m_useHoverPID && !m_usePID
3788
3789 /// Dynamics RotLookAt =================================================================================
3790 if (m_useAPID)
3791 {
3792 // RotLookAt, apparently overrides all other rotation sources. Inputs:
3793 // Quaternion m_APIDTarget
3794 // float m_APIDStrength // From SL experiments, this is the time to get there
3795 // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
3796 // Also in SL the mass of the object has no effect on time to get there.
3797 // Factors:
3798 // get present body rotation
3799 float limit = 1.0f;
3800 float scaler = 50f; // adjusts damping time
3801 float RLAservo = 0f;
3802
3803 d.Quaternion rot = d.BodyGetQuaternion(Body);
3804 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
3805 Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget;
3806 float diff_angle;
3807 Vector3 diff_axis;
3808 rot_diff.GetAxisAngle(out diff_axis, out diff_angle);
3809 diff_axis.Normalize();
3810 if(diff_angle > 0.01f) // diff_angle is always +ve
3811 {
3812// PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z);
3813 Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
3814 rotforce = rotforce * rotq;
3815 if(diff_angle > limit) diff_angle = limit; // cap the rotate rate
3816// RLAservo = timestep / m_APIDStrength * m_mass * scaler;
3817 // rotforce = rotforce * RLAservo * diff_angle ;
3818 // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z);
3819 RLAservo = timestep / m_APIDStrength * scaler;
3820 rotforce = rotforce * RLAservo * diff_angle ;
3821 /*
3822 if (m_angularEnable.X == 0)
3823 rotforce.X = 0;
3824 if (m_angularEnable.Y == 0)
3825 rotforce.Y = 0;
3826 if (m_angularEnable.Z == 0)
3827 rotforce.Z = 0;
3828 */
3829 d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z);
3830//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo);
3831 }
3832//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle);
3833 } // end m_useAPID
3834
3835 /// Dynamics Apply Forces ===================================================================================
3836 fx *= m_mass;
3837 fy *= m_mass;
3838 //fz *= m_mass;
3839
3840 fx += m_force.X;
3841 fy += m_force.Y;
3842 fz += m_force.Z;
3843
3844 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
3845 if (fx != 0 || fy != 0 || fz != 0)
3846 {
3847 //m_taintdisable = true;
3848 //base.RaiseOutOfBounds(Position);
3849 //d.BodySetLinearVel(Body, fx, fy, 0f);
3850 if (!d.BodyIsEnabled(Body))
3851 {
3852 // A physical body at rest on a surface will auto-disable after a while,
3853 // this appears to re-enable it incase the surface it is upon vanishes,
3854 // and the body should fall again.
3855 d.BodySetLinearVel(Body, 0f, 0f, 0f);
3856 d.BodySetForce(Body, 0, 0, 0);
3857 enableBodySoft();
3858 }
3859
3860 // 35x10 = 350n times the mass per second applied maximum.
3861 float nmax = 35f * m_mass;
3862 float nmin = -35f * m_mass;
3863
3864
3865 if (fx > nmax)
3866 fx = nmax;
3867 if (fx < nmin)
3868 fx = nmin;
3869 if (fy > nmax)
3870 fy = nmax;
3871 if (fy < nmin)
3872 fy = nmin;
3873 d.BodyAddForce(Body, fx, fy, fz);
3874//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
3875 } // end apply forces
3876 } // end Dynamics
3877
3878/* obsolete?
3879 else
3880 { // is not physical, or is not a body or is selected
3881 // from old UpdatePositionAndVelocity, ... Not a body.. so Make sure the client isn't interpolating
3882 _velocity.X = 0;
3883 _velocity.Y = 0;
3884 _velocity.Z = 0;
3885
3886 _acceleration.X = 0;
3887 _acceleration.Y = 0;
3888 _acceleration.Z = 0;
3889
3890 m_rotationalVelocity.X = 0;
3891 m_rotationalVelocity.Y = 0;
3892 m_rotationalVelocity.Z = 0;
3893 _zeroFlag = true;
3894 return;
3895 }
3896 */
3897 } // end root prims
3898
3899 } // end Move()
3900 } // end class
3901}
generated by cgit v1.1 at 2024-12-15 10:44:32 +0000