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