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