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