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