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