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