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