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