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.cs3917
1 files changed, 3917 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..793774a
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -0,0 +1,3917 @@
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 int fence = _parent_scene.geomRegionFence;
3005
3006 float border_limit = 0.05f; // original limit
3007 if (fence == 1) border_limit = 0.5f; // bounce point
3008
3009 frcount++; // used to limit debug comment output
3010 if (frcount > 50)
3011 frcount = 0;
3012
3013 if(revcount > 0) revcount--;
3014
3015 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // Only move root prims.
3016 {
3017 // Old public void UpdatePositionAndVelocity(), more accuratley calculated here
3018 bool lastZeroFlag = _zeroFlag; // was it stopped
3019
3020 d.Vector3 vec = d.BodyGetPosition(Body);
3021 Vector3 l_position = Vector3.Zero;
3022 l_position.X = vec.X;
3023 l_position.Y = vec.Y;
3024 l_position.Z = vec.Z;
3025 m_lastposition = _position;
3026 _position = l_position;
3027
3028 d.Quaternion ori = d.BodyGetQuaternion(Body);
3029 // Quaternion l_orientation = Quaternion.Identity;
3030 _orientation.X = ori.X;
3031 _orientation.Y = ori.Y;
3032 _orientation.Z = ori.Z;
3033 _orientation.W = ori.W;
3034 m_lastorientation = _orientation;
3035
3036 d.Vector3 vel = d.BodyGetLinearVel(Body);
3037 m_lastVelocity = _velocity;
3038 _velocity.X = vel.X;
3039 _velocity.Y = vel.Y;
3040 _velocity.Z = vel.Z;
3041 _acceleration = ((_velocity - m_lastVelocity) / timestep);
3042
3043 d.Vector3 torque = d.BodyGetTorque(Body);
3044 _torque = new Vector3(torque.X, torque.Y, torque.Z);
3045
3046 base.RequestPhysicsterseUpdate();
3047
3048//Console.WriteLine("Move {0} at {1}", m_primName, l_position);
3049
3050 // Check if outside region
3051 // In Scene.cs/CrossPrimGroupIntoNewRegion the object is checked for 0.1M from border!
3052 if (l_position.X > ((float)_parent_scene.WorldExtents.X - border_limit))
3053 {
3054 l_position.X = ((float)_parent_scene.WorldExtents.X - border_limit);
3055 outside = 1;
3056 }
3057
3058 if (l_position.X < border_limit)
3059 {
3060 l_position.X = border_limit;
3061 outside = 2;
3062 }
3063 if (l_position.Y > ((float)_parent_scene.WorldExtents.Y - border_limit))
3064 {
3065 l_position.Y = ((float)_parent_scene.WorldExtents.Y - border_limit);
3066 outside = 3;
3067 }
3068
3069 if (l_position.Y < border_limit)
3070 {
3071 l_position.Y = border_limit;
3072 outside = 4;
3073 }
3074
3075 if (outside > 0)
3076 {
3077//Console.WriteLine(" fence = {0}",fence);
3078
3079//Console.WriteLine("Border {0}", l_position);
3080 if (fence == 1) // bounce object off boundary
3081 {
3082 if (revcount == 0)
3083 {
3084 if (outside < 3)
3085 {
3086 _velocity.X = -_velocity.X;
3087 }
3088 else
3089 {
3090 _velocity.Y = -_velocity.Y;
3091 }
3092 if (m_type != Vehicle.TYPE_NONE) Halt();
3093 _position = l_position;
3094 m_taintposition = _position;
3095 m_lastVelocity = _velocity;
3096 _acceleration = Vector3.Zero;
3097 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
3098 d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
3099 base.RequestPhysicsterseUpdate();
3100
3101 revcount = 25; // wait for object to move away from border
3102 }
3103 } // else old crossing mode
3104 else if (m_crossingfailures < failureLimit)
3105 { // keep trying to cross?
3106 _position = l_position;
3107 //_parent_scene.remActivePrim(this);
3108 if (_parent == null) base.RequestPhysicsterseUpdate();
3109 return; // Dont process any other motion?
3110 }
3111 else
3112 { // Too many tries
3113 if (_parent == null) base.RaiseOutOfBounds(l_position);
3114//Console.WriteLine("ROOB 2");
3115
3116 return; // Dont process any other motion?
3117 } // end various methods
3118 } // end outside region horizontally
3119
3120
3121 if (l_position.Z < 0)
3122 {
3123 // This is so prim that get lost underground don't fall forever and suck up
3124 //
3125 // Sim resources and memory.
3126 // Disables the prim's movement physics....
3127 // It's a hack and will generate a console message if it fails.
3128
3129 //IsPhysical = false;
3130 if (_parent == null) base.RaiseOutOfBounds(_position);
3131//Console.WriteLine("ROOB 3");
3132
3133
3134 _acceleration.X = 0; // This stuff may stop client display but it has no
3135 _acceleration.Y = 0; // effect on the object in phys engine!
3136 _acceleration.Z = 0;
3137
3138 _velocity.X = 0;
3139 _velocity.Y = 0;
3140 _velocity.Z = 0;
3141 m_rotationalVelocity.X = 0;
3142 m_rotationalVelocity.Y = 0;
3143 m_rotationalVelocity.Z = 0;
3144
3145 if (_parent == null) base.RequestPhysicsterseUpdate();
3146
3147 m_throttleUpdates = false;
3148 throttleCounter = 0;
3149 _zeroFlag = true;
3150 //outofBounds = true;
3151 } // end neg Z check
3152
3153 // Is it moving?
3154 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
3155 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
3156 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
3157 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001)) // KF 0.01 is far to large
3158 {
3159 _zeroFlag = true;
3160 m_throttleUpdates = false;
3161 }
3162 else
3163 {
3164 //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
3165 _zeroFlag = false;
3166 m_lastUpdateSent = false;
3167 //m_throttleUpdates = false;
3168 }
3169
3170 if (_zeroFlag)
3171 { // Its stopped
3172 _velocity.X = 0.0f;
3173 _velocity.Y = 0.0f;
3174 _velocity.Z = 0.0f;
3175
3176 _acceleration.X = 0;
3177 _acceleration.Y = 0;
3178 _acceleration.Z = 0;
3179
3180 m_rotationalVelocity.X = 0;
3181 m_rotationalVelocity.Y = 0;
3182 m_rotationalVelocity.Z = 0;
3183 if (!m_lastUpdateSent)
3184 {
3185 m_throttleUpdates = false;
3186 throttleCounter = 0;
3187 if (_parent == null)
3188 {
3189 base.RequestPhysicsterseUpdate();
3190 }
3191
3192 m_lastUpdateSent = true;
3193 }
3194 }
3195 else
3196 { // Its moving
3197 if (lastZeroFlag != _zeroFlag)
3198 {
3199 if (_parent == null)
3200 {
3201 base.RequestPhysicsterseUpdate();
3202 }
3203 }
3204 m_lastUpdateSent = false;
3205 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
3206 {
3207 if (_parent == null)
3208 {
3209 base.RequestPhysicsterseUpdate();
3210 }
3211 }
3212 else
3213 {
3214 throttleCounter++;
3215 }
3216 }
3217 m_lastposition = l_position;
3218
3219 /// End UpdatePositionAndVelocity insert
3220
3221
3222 // Rotation lock =====================================
3223 if(m_rotateEnableUpdate)
3224 {
3225 // Snapshot current angles, set up Amotor(s)
3226 m_rotateEnableUpdate = false;
3227 m_rotateEnable = m_rotateEnableRequest;
3228Console.WriteLine("RotEnable {0} = {1}",m_primName, m_rotateEnable);
3229
3230 if (Amotor != IntPtr.Zero)
3231 {
3232 d.JointDestroy(Amotor);
3233 Amotor = IntPtr.Zero;
3234Console.WriteLine("Old Amotor Destroyed");
3235 }
3236
3237 if (!m_rotateEnable.ApproxEquals(Vector3.One, 0.003f))
3238 { // not all are enabled
3239 d.Quaternion r = d.BodyGetQuaternion(Body);
3240 Quaternion locrot = new Quaternion(r.X, r.Y, r.Z, r.W);
3241 // extract the axes vectors
3242 Vector3 vX = new Vector3(1f,0f,0f);
3243 Vector3 vY = new Vector3(0f,1f,0f);
3244 Vector3 vZ = new Vector3(0f,0f,1f);
3245 vX = vX * locrot;
3246 vY = vY * locrot;
3247 vZ = vZ * locrot;
3248 // snapshot the current angle vectors
3249 m_lockX = vX;
3250 m_lockY = vY;
3251 m_lockZ = vZ;
3252 // m_lockRot = locrot;
3253 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
3254 d.JointAttach(Amotor, Body, IntPtr.Zero);
3255 d.JointSetAMotorMode(Amotor, 0); // User mode??
3256Console.WriteLine("New Amotor Created for {0}", m_primName);
3257
3258 float axisnum = 3; // how many to lock
3259 axisnum = (axisnum - (m_rotateEnable.X + m_rotateEnable.Y + m_rotateEnable.Z));
3260 d.JointSetAMotorNumAxes(Amotor,(int)axisnum);
3261Console.WriteLine("AxisNum={0}",(int)axisnum);
3262
3263 int i = 0;
3264
3265 if (m_rotateEnable.X == 0)
3266 {
3267 d.JointSetAMotorAxis(Amotor, i, 0, m_lockX.X, m_lockX.Y, m_lockX.Z);
3268Console.WriteLine("AxisX {0} set to {1}", i, m_lockX);
3269 i++;
3270 }
3271
3272 if (m_rotateEnable.Y == 0)
3273 {
3274 d.JointSetAMotorAxis(Amotor, i, 0, m_lockY.X, m_lockY.Y, m_lockY.Z);
3275Console.WriteLine("AxisY {0} set to {1}", i, m_lockY);
3276 i++;
3277 }
3278
3279 if (m_rotateEnable.Z == 0)
3280 {
3281 d.JointSetAMotorAxis(Amotor, i, 0, m_lockZ.X, m_lockZ.Y, m_lockZ.Z);
3282Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ);
3283 i++;
3284 }
3285
3286 // These lowstops and high stops are effectively (no wiggle room)
3287 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, 0f);
3288 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, 0f);
3289 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, 0f);
3290 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
3291 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
3292 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
3293 d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 0f);
3294 d.JointSetAMotorParam(Amotor, (int) dParam.Vel3, 0f);
3295 d.JointSetAMotorParam(Amotor, (int) dParam.Vel2, 0f);
3296 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f);
3297 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f);
3298 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f);
3299 } // else none are locked
3300 } // end Rotation Update
3301
3302
3303 // VEHICLE processing ==========================================
3304 if (m_type != Vehicle.TYPE_NONE)
3305 {
3306 // get body attitude
3307 d.Quaternion rot = d.BodyGetQuaternion(Body);
3308 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
3309 Quaternion irotq = Quaternion.Inverse(rotq);
3310
3311 // VEHICLE Linear Motion
3312 d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame
3313 Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z);
3314 m_lLinObjectVel = vel_now * irotq;
3315
3316 if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate
3317 {
3318 if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f)
3319 {
3320 float decayfactor = m_linearMotorDecayTimescale/timestep;
3321 Vector3 decayAmount = (m_lLinMotorDVel/decayfactor);
3322 m_lLinMotorDVel -= decayAmount;
3323 }
3324 else
3325 {
3326 float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale)));
3327 Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep;
3328 m_lLinMotorDVel -= decel;
3329 }
3330 if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
3331 {
3332 m_lLinMotorDVel = Vector3.Zero;
3333 }
3334
3335 /* else
3336 {
3337 if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X;
3338 if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y;
3339 if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z;
3340 } */
3341 } // end linear motor decay
3342
3343 if ( (! m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
3344 {
3345 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
3346 if (m_linearMotorTimescale < 300.0f)
3347 {
3348 Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel;
3349 float linfactor = m_linearMotorTimescale/timestep;
3350 Vector3 attackAmount = (attack_error/linfactor) * 1.3f;
3351 m_lLinObjectVel += attackAmount;
3352 }
3353 if (m_linearFrictionTimescale.X < 300.0f)
3354 {
3355 float fricfactor = m_linearFrictionTimescale.X / timestep;
3356 float fricX = m_lLinObjectVel.X / fricfactor;
3357 m_lLinObjectVel.X -= fricX;
3358 }
3359 if (m_linearFrictionTimescale.Y < 300.0f)
3360 {
3361 float fricfactor = m_linearFrictionTimescale.Y / timestep;
3362 float fricY = m_lLinObjectVel.Y / fricfactor;
3363 m_lLinObjectVel.Y -= fricY;
3364 }
3365 if (m_linearFrictionTimescale.Z < 300.0f)
3366 {
3367 float fricfactor = m_linearFrictionTimescale.Z / timestep;
3368 float fricZ = m_lLinObjectVel.Z / fricfactor;
3369 m_lLinObjectVel.Z -= fricZ;
3370 }
3371 }
3372 m_wLinObjectVel = m_lLinObjectVel * rotq;
3373
3374 // Gravity and Buoyancy
3375 Vector3 grav = Vector3.Zero;
3376 if(m_VehicleBuoyancy < 1.0f)
3377 {
3378 // There is some gravity, make a gravity force vector
3379 // that is applied after object velocity.
3380 d.Mass objMass;
3381 d.BodyGetMass(Body, out objMass);
3382 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
3383 grav.Z = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force
3384 } // else its 1.0, no gravity.
3385
3386 // Hovering
3387 if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
3388 {
3389 // We should hover, get the target height
3390 d.Vector3 pos = d.BodyGetPosition(Body);
3391 if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
3392 {
3393 m_VhoverTargetHeight = _parent_scene.GetWaterLevel() + m_VhoverHeight;
3394 }
3395 else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
3396 {
3397 m_VhoverTargetHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
3398 }
3399 else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
3400 {
3401 m_VhoverTargetHeight = m_VhoverHeight;
3402 }
3403
3404 if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
3405 {
3406 // If body is aready heigher, use its height as target height
3407 if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
3408 }
3409
3410// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
3411// m_VhoverTimescale = 0f; // time to acheive height
3412// timestep is time since last frame,in secs
3413 float herr0 = pos.Z - m_VhoverTargetHeight;
3414 // Replace Vertical speed with correction figure if significant
3415 if(Math.Abs(herr0) > 0.01f )
3416 {
3417 //? d.Mass objMass;
3418 //? d.BodyGetMass(Body, out objMass);
3419 m_wLinObjectVel.Z = - ( (herr0 * timestep * 50.0f) / m_VhoverTimescale);
3420 //KF: m_VhoverEfficiency is not yet implemented
3421 }
3422 else
3423 {
3424 m_wLinObjectVel.Z = 0f;
3425 }
3426 }
3427 else
3428 { // not hovering
3429 if (m_wLinObjectVel.Z == 0f)
3430 { // Gravity rules
3431 m_wLinObjectVel.Z = vel_now.Z;
3432 } // else the motor has it
3433 }
3434 linvel = m_wLinObjectVel;
3435
3436 // Vehicle Linear Motion done =======================================
3437 // Apply velocity
3438 d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z);
3439 // apply gravity force
3440 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
3441//if(frcount == 0) Console.WriteLine("Grav {0}", grav);
3442 // end MoveLinear()
3443
3444
3445 // MoveAngular
3446 /*
3447 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
3448
3449 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
3450 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
3451 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
3452
3453 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
3454 private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body
3455 */
3456//if(frcount == 0) Console.WriteLine("MoveAngular ");
3457
3458 d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body);
3459 Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z);
3460 angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation
3461
3462//if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel);
3463
3464 // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack.
3465 float atk_decayfactor = 23.0f / (m_angularMotorTimescale * timestep);
3466 m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor;
3467 // Decay Angular Motor 2.
3468 if (m_angularMotorDecayTimescale < 300.0f)
3469 {
3470 if ( Vector3.Mag(m_angularMotorDVel) < 1.0f)
3471 {
3472 float decayfactor = (m_angularMotorDecayTimescale)/timestep;
3473 Vector3 decayAmount = (m_angularMotorDVel/decayfactor);
3474 m_angularMotorDVel -= decayAmount;
3475 }
3476 else
3477 {
3478 Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale;
3479 m_angularMotorDVel -= decel;
3480 }
3481
3482 if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
3483 {
3484 m_angularMotorDVel = Vector3.Zero;
3485 }
3486 else
3487 {
3488 if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X;
3489 if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y;
3490 if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z;
3491 }
3492 } // end decay angular motor
3493//if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel);
3494
3495//if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel);
3496
3497 if ( (! m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
3498 { // if motor or object have motion
3499 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
3500
3501 if (m_angularMotorTimescale < 300.0f)
3502 {
3503 Vector3 attack_error = m_angularMotorDVel - angObjectVel;
3504 float angfactor = m_angularMotorTimescale/timestep;
3505 Vector3 attackAmount = (attack_error/angfactor);
3506 angObjectVel += attackAmount;
3507//if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount);
3508//if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel);
3509 }
3510
3511 angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep);
3512 angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep);
3513 angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep);
3514 } // else no signif. motion
3515
3516//if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel);
3517 // Bank section tba
3518 // Deflection section tba
3519//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel);
3520
3521
3522 /* // Rotation Axis Disables:
3523 if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f))
3524 {
3525 if (m_angularEnable.X == 0)
3526 angObjectVel.X = 0f;
3527 if (m_angularEnable.Y == 0)
3528 angObjectVel.Y = 0f;
3529 if (m_angularEnable.Z == 0)
3530 angObjectVel.Z = 0f;
3531 }
3532 */
3533 angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation
3534
3535 // Vertical attractor section
3536 Vector3 vertattr = Vector3.Zero;
3537
3538 if(m_verticalAttractionTimescale < 300)
3539 {
3540 float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep);
3541 // make a vector pointing up
3542 Vector3 verterr = Vector3.Zero;
3543 verterr.Z = 1.0f;
3544 // rotate it to Body Angle
3545 verterr = verterr * rotq;
3546 // 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.
3547 // 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
3548 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
3549
3550 if (verterr.Z < 0.0f)
3551 { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to
3552 // vertical, BUT for some reason a z-rotation is imparted to the object. TBI.
3553//Console.WriteLine("InvertFlip");
3554 verterr.X = 2.0f - verterr.X;
3555 verterr.Y = 2.0f - verterr.Y;
3556 }
3557 verterr *= 0.5f;
3558 // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt)
3559 Vector3 xyav = angObjectVel;
3560 xyav.Z = 0.0f;
3561 if ((!xyav.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f))
3562 {
3563 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
3564 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
3565 vertattr.X = verterr.Y;
3566 vertattr.Y = - verterr.X;
3567 vertattr.Z = 0f;
3568//if(frcount == 0) Console.WriteLine("VAerr=" + verterr);
3569
3570 // scaling appears better usingsquare-law
3571 float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
3572 float bounce = 1.0f - damped;
3573 // 0 = crit damp, 1 = bouncy
3574 float oavz = angObjectVel.Z; // retain z velocity
3575 // time-scaled correction, which sums, therefore is bouncy:
3576 angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce;
3577 // damped, good @ < 90:
3578 angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped);
3579 angObjectVel.Z = oavz;
3580//if(frcount == 0) Console.WriteLine("VA+");
3581//Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel);
3582 }
3583 else
3584 {
3585 // else error is very small
3586 angObjectVel.X = 0f;
3587 angObjectVel.Y = 0f;
3588//if(frcount == 0) Console.WriteLine("VA0");
3589 }
3590 } // else vertical attractor is off
3591//if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel);
3592
3593
3594 m_lastAngularVelocity = angObjectVel;
3595 // apply Angular Velocity to body
3596 d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
3597//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity);
3598
3599 } // end VEHICLES
3600 else
3601 {
3602 // Dyamics (NON-'VEHICLES') are dealt with here ================================================================
3603
3604 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
3605
3606 /// Dynamics Buoyancy
3607 //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle.
3608 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
3609 // NB Prims in ODE are no subject to global gravity
3610 // This should only affect gravity operations
3611
3612 float m_mass = CalculateMass();
3613 // calculate z-force due togravity on object.
3614 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass
3615
3616 if ((m_usePID) && (m_PIDTau > 0.0f)) // Dynamics llMoveToTarget.
3617 {
3618 fz = 0; // llMoveToTarget ignores gravity.
3619 // it also ignores mass of object, and any physical resting on it.
3620 // Vector3 m_PIDTarget is where we are going
3621 // float m_PIDTau is time to get there
3622 fx = 0;
3623 fy = 0;
3624 d.Vector3 pos = d.BodyGetPosition(Body);
3625 Vector3 error = new Vector3(
3626 (m_PIDTarget.X - pos.X),
3627 (m_PIDTarget.Y - pos.Y),
3628 (m_PIDTarget.Z - pos.Z));
3629 if (error.ApproxEquals(Vector3.Zero,0.01f))
3630 { // Very close, Jump there and quit move
3631 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3632 _target_velocity = Vector3.Zero;
3633 d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
3634 }
3635 else
3636 {
3637 float scale = 50.0f * timestep / m_PIDTau;
3638 if ((error.ApproxEquals(Vector3.Zero,0.5f)) && (_target_velocity != Vector3.Zero))
3639 {
3640 // Nearby, quit update of velocity
3641 }
3642 else
3643 { // Far, calc damped velocity
3644 _target_velocity = error * scale;
3645 }
3646 d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
3647 }
3648 } // end PID MoveToTarget
3649
3650 /* Original OS implementation: Does not work correctly as another phys object resting on THIS object purturbs its position.
3651 This is incorrect behavior. llMoveToTarget must move the Body no matter what phys object is resting on it.
3652
3653 //if (!d.BodyIsEnabled(Body))
3654 //d.BodySetForce(Body, 0f, 0f, 0f);
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) && (m_PIDTau != 0))
3662 {
3663 //PID_G = PID_G / m_PIDTau;
3664 m_PIDTau = 1;
3665 }
3666
3667 if ((PID_G - m_PIDTau) <= 0)
3668 {
3669 PID_G = m_PIDTau + 1;
3670 }
3671 //PidStatus = true;
3672
3673 // PhysicsVector vec = new PhysicsVector();
3674// d.Vector3 vel = d.BodyGetLinearVel(Body);
3675
3676 d.Vector3 pos = d.BodyGetPosition(Body);
3677 _target_velocity =
3678 new Vector3(
3679 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
3680 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
3681 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
3682 );
3683
3684if(frcount == 0) Console.WriteLine("PID {0} b={1} fz={2} vel={3}", m_primName, m_buoyancy, fz, _target_velocity);
3685 // if velocity is zero, use position control; otherwise, velocity control
3686
3687 if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
3688 {
3689 // keep track of where we stopped. No more slippin' & slidin'
3690
3691 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3692 // react to the physics scene by moving it's position.
3693 // Avatar to Avatar collisions
3694 // Prim to avatar collisions
3695
3696 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
3697 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
3698 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
3699 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3700 d.BodySetLinearVel(Body, 0, 0, 0);
3701 d.BodyAddForce(Body, 0, 0, fz);
3702 // return;
3703 }
3704 else
3705 {
3706 _zeroFlag = false;
3707
3708 // We're flying and colliding with something
3709 fx = ((_target_velocity.X) - vel.X) * (PID_D);
3710 fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
3711
3712 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
3713
3714 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
3715 }
3716 } // end if (m_usePID)
3717 End of old PID system */
3718
3719
3720 /// Dynamics Hover ===================================================================================
3721 // Hover PID Controller can only run if the PIDcontroller is not in use.
3722 if (m_useHoverPID && !m_usePID)
3723 {
3724//Console.WriteLine("Hover " + m_primName);
3725
3726 // If we're using the PID controller, then we have no gravity
3727 fz = (-1 * _parent_scene.gravityz) * m_mass;
3728
3729 // no lock; for now it's only called from within Simulate()
3730
3731 // If the PID Controller isn't active then we set our force
3732 // calculating base velocity to the current position
3733
3734 if ((m_PIDTau < 1))
3735 {
3736 PID_G = PID_G / m_PIDTau;
3737 }
3738
3739 if ((PID_G - m_PIDTau) <= 0)
3740 {
3741 PID_G = m_PIDTau + 1;
3742 }
3743
3744
3745 // Where are we, and where are we headed?
3746 d.Vector3 pos = d.BodyGetPosition(Body);
3747// d.Vector3 vel = d.BodyGetLinearVel(Body);
3748
3749
3750 // Non-Vehicles have a limited set of Hover options.
3751 // determine what our target height really is based on HoverType
3752 switch (m_PIDHoverType)
3753 {
3754 case PIDHoverType.Ground:
3755 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3756 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3757 break;
3758 case PIDHoverType.GroundAndWater:
3759 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3760 m_waterHeight = _parent_scene.GetWaterLevel();
3761 if (m_groundHeight > m_waterHeight)
3762 {
3763 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3764 }
3765 else
3766 {
3767 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
3768 }
3769 break;
3770
3771 } // end switch (m_PIDHoverType)
3772
3773
3774 _target_velocity =
3775 new Vector3(0.0f, 0.0f,
3776 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
3777 );
3778
3779 // if velocity is zero, use position control; otherwise, velocity control
3780
3781 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
3782 {
3783 // keep track of where we stopped. No more slippin' & slidin'
3784
3785 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3786 // react to the physics scene by moving it's position.
3787 // Avatar to Avatar collisions
3788 // Prim to avatar collisions
3789 d.Vector3 dlinvel = vel;
3790
3791 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
3792 d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z);
3793 d.BodyAddForce(Body, 0, 0, fz);
3794 //KF this prevents furthur motions return;
3795 }
3796 else
3797 {
3798 _zeroFlag = false;
3799
3800 // We're flying and colliding with something
3801 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
3802 }
3803 } // end m_useHoverPID && !m_usePID
3804
3805 /// Dynamics RotLookAt =================================================================================
3806 if (m_useAPID)
3807 {
3808 // RotLookAt, apparently overrides all other rotation sources. Inputs:
3809 // Quaternion m_APIDTarget
3810 // float m_APIDStrength // From SL experiments, this is the time to get there
3811 // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
3812 // Also in SL the mass of the object has no effect on time to get there.
3813 // Factors:
3814 // get present body rotation
3815 float limit = 1.0f;
3816 float scaler = 50f; // adjusts damping time
3817 float RLAservo = 0f;
3818
3819 d.Quaternion rot = d.BodyGetQuaternion(Body);
3820 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
3821 Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget;
3822 float diff_angle;
3823 Vector3 diff_axis;
3824 rot_diff.GetAxisAngle(out diff_axis, out diff_angle);
3825 diff_axis.Normalize();
3826 if(diff_angle > 0.01f) // diff_angle is always +ve
3827 {
3828// PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z);
3829 Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
3830 rotforce = rotforce * rotq;
3831 if(diff_angle > limit) diff_angle = limit; // cap the rotate rate
3832// RLAservo = timestep / m_APIDStrength * m_mass * scaler;
3833 // rotforce = rotforce * RLAservo * diff_angle ;
3834 // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z);
3835 RLAservo = timestep / m_APIDStrength * scaler;
3836 rotforce = rotforce * RLAservo * diff_angle ;
3837 /*
3838 if (m_angularEnable.X == 0)
3839 rotforce.X = 0;
3840 if (m_angularEnable.Y == 0)
3841 rotforce.Y = 0;
3842 if (m_angularEnable.Z == 0)
3843 rotforce.Z = 0;
3844 */
3845 d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z);
3846//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo);
3847 }
3848//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle);
3849 } // end m_useAPID
3850
3851 /// Dynamics Apply Forces ===================================================================================
3852 fx *= m_mass;
3853 fy *= m_mass;
3854 //fz *= m_mass;
3855
3856 fx += m_force.X;
3857 fy += m_force.Y;
3858 fz += m_force.Z;
3859
3860 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
3861 if (fx != 0 || fy != 0 || fz != 0)
3862 {
3863 //m_taintdisable = true;
3864 //base.RaiseOutOfBounds(Position);
3865 //d.BodySetLinearVel(Body, fx, fy, 0f);
3866 if (!d.BodyIsEnabled(Body))
3867 {
3868 // A physical body at rest on a surface will auto-disable after a while,
3869 // this appears to re-enable it incase the surface it is upon vanishes,
3870 // and the body should fall again.
3871 d.BodySetLinearVel(Body, 0f, 0f, 0f);
3872 d.BodySetForce(Body, 0, 0, 0);
3873 enableBodySoft();
3874 }
3875
3876 // 35x10 = 350n times the mass per second applied maximum.
3877 float nmax = 35f * m_mass;
3878 float nmin = -35f * m_mass;
3879
3880
3881 if (fx > nmax)
3882 fx = nmax;
3883 if (fx < nmin)
3884 fx = nmin;
3885 if (fy > nmax)
3886 fy = nmax;
3887 if (fy < nmin)
3888 fy = nmin;
3889 d.BodyAddForce(Body, fx, fy, fz);
3890//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
3891 } // end apply forces
3892 } // end Dynamics
3893
3894/* obsolete?
3895 else
3896 { // is not physical, or is not a body or is selected
3897 // from old UpdatePositionAndVelocity, ... Not a body.. so Make sure the client isn't interpolating
3898 _velocity.X = 0;
3899 _velocity.Y = 0;
3900 _velocity.Z = 0;
3901
3902 _acceleration.X = 0;
3903 _acceleration.Y = 0;
3904 _acceleration.Z = 0;
3905
3906 m_rotationalVelocity.X = 0;
3907 m_rotationalVelocity.Y = 0;
3908 m_rotationalVelocity.Z = 0;
3909 _zeroFlag = true;
3910 return;
3911 }
3912 */
3913 } // end root prims
3914
3915 } // end Move()
3916 } // end class
3917}
generated by cgit v1.1 at 2024-11-10 16:32:38 +0000