aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/ChOdePlugin
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/ChOdePlugin')
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs2428
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs36
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs353
3 files changed, 1777 insertions, 1040 deletions
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
index a68dcb7..80c1277 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -22,11 +22,13 @@
22 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 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. 23 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
24 * 24 *
25 * Revised March 5th 2010 by Kitto Flora. ODEDynamics.cs 25 * Revised March 5th 2010 by Kitto Flora. ODEDynamics.cs
26 * Ubit 2012
26 * rolled into ODEPrim.cs 27 * rolled into ODEPrim.cs
27 */ 28 */
28 29
29using System; 30using System;
31using System.IO;
30using System.Collections.Generic; 32using System.Collections.Generic;
31using System.Reflection; 33using System.Reflection;
32using System.Runtime.InteropServices; 34using System.Runtime.InteropServices;
@@ -37,7 +39,6 @@ using Ode.NET;
37using OpenSim.Framework; 39using OpenSim.Framework;
38using OpenSim.Region.Physics.Manager; 40using OpenSim.Region.Physics.Manager;
39 41
40
41namespace OpenSim.Region.Physics.OdePlugin 42namespace OpenSim.Region.Physics.OdePlugin
42{ 43{
43 /// <summary> 44 /// <summary>
@@ -48,6 +49,11 @@ namespace OpenSim.Region.Physics.OdePlugin
48 { 49 {
49 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); 50 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
50 51
52 public class SerialControl
53 {
54 public object alock = new object();
55 public byte[] data = new byte[0];
56 }
51 private Vector3 _position; 57 private Vector3 _position;
52 private Vector3 _velocity; 58 private Vector3 _velocity;
53 private Vector3 _torque; 59 private Vector3 _torque;
@@ -80,12 +86,12 @@ namespace OpenSim.Region.Physics.OdePlugin
80 private float PID_D = 35f; 86 private float PID_D = 35f;
81 private float PID_G = 25f; 87 private float PID_G = 25f;
82 private bool m_usePID = false; 88 private bool m_usePID = false;
83 89
84 private Quaternion m_APIDTarget = new Quaternion(); 90 private Quaternion m_APIDTarget = new Quaternion();
85 private float m_APIDStrength = 0.5f; 91 private float m_APIDStrength = 0.5f;
86 private float m_APIDDamping = 0.5f; 92 private float m_APIDDamping = 0.5f;
87 private bool m_useAPID = false; 93 private bool m_useAPID = false;
88 private float m_APIDdamper = 1.0f; 94 private float m_APIDdamper = 1.0f;
89 95
90 // These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), 96 // These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
91 // do not confuse with VEHICLE HOVER 97 // do not confuse with VEHICLE HOVER
@@ -112,7 +118,7 @@ namespace OpenSim.Region.Physics.OdePlugin
112 private bool m_taintPhysics; 118 private bool m_taintPhysics;
113 private bool m_collidesLand = true; 119 private bool m_collidesLand = true;
114 private bool m_collidesWater; 120 private bool m_collidesWater;
115 public bool m_returnCollisions; 121 // public bool m_returnCollisions;
116 122
117 // Default we're a Geometry 123 // Default we're a Geometry
118 private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); 124 private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
@@ -144,7 +150,7 @@ namespace OpenSim.Region.Physics.OdePlugin
144 private OdeScene _parent_scene; 150 private OdeScene _parent_scene;
145 public IntPtr m_targetSpace = IntPtr.Zero; 151 public IntPtr m_targetSpace = IntPtr.Zero;
146 public IntPtr prim_geom; 152 public IntPtr prim_geom;
147// public IntPtr prev_geom; 153 // public IntPtr prev_geom;
148 public IntPtr _triMeshData; 154 public IntPtr _triMeshData;
149 155
150 private IntPtr _linkJointGroup = IntPtr.Zero; 156 private IntPtr _linkJointGroup = IntPtr.Zero;
@@ -163,8 +169,8 @@ namespace OpenSim.Region.Physics.OdePlugin
163 private int throttleCounter; 169 private int throttleCounter;
164 public int m_interpenetrationcount; 170 public int m_interpenetrationcount;
165 public float m_collisionscore; 171 public float m_collisionscore;
166 public int m_roundsUnderMotionThreshold; 172 // public int m_roundsUnderMotionThreshold;
167 private int m_crossingfailures; 173 // private int m_crossingfailures;
168 174
169 public bool m_outofBounds; 175 public bool m_outofBounds;
170 private float m_density = 10.000006836f; // Aluminum g/cm3; 176 private float m_density = 10.000006836f; // Aluminum g/cm3;
@@ -185,75 +191,525 @@ namespace OpenSim.Region.Physics.OdePlugin
185 public volatile bool childPrim; 191 public volatile bool childPrim;
186 192
187 internal int m_material = (int)Material.Wood; 193 internal int m_material = (int)Material.Wood;
188
189 private int frcount = 0; // Used to limit dynamics debug output to
190 private int revcount = 0; // Reverse motion while > 0
191 194
192 private IntPtr m_body = IntPtr.Zero; 195 private IntPtr m_body = IntPtr.Zero;
193 196
194 // Vehicle properties ============================================================================================ 197 // Vehicle properties ============================================================================================
195 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind 198 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
196 // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier 199 // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
197 private VehicleFlag m_flags = (VehicleFlag) 0; // Bit settings: 200 private VehicleFlag m_flags = (VehicleFlag)0; // Bit settings:
198 // HOVER_TERRAIN_ONLY 201 // HOVER_TERRAIN_ONLY
199 // HOVER_GLOBAL_HEIGHT 202 // HOVER_GLOBAL_HEIGHT
200 // NO_DEFLECTION_UP 203 // NO_DEFLECTION_UP
201 // HOVER_WATER_ONLY 204 // HOVER_WATER_ONLY
202 // HOVER_UP_ONLY 205 // HOVER_UP_ONLY
203 // LIMIT_MOTOR_UP 206 // LIMIT_MOTOR_UP
204 // LIMIT_ROLL_ONLY 207 // LIMIT_ROLL_ONLY
205 208
206 // Linear properties 209 // Linear properties
207 private Vector3 m_linearMotorDirection = Vector3.Zero; // (was m_linearMotorDirectionLASTSET) the (local) Velocity 210 private Vector3 m_linearMotorDirection = Vector3.Zero; // (was m_linearMotorDirectionLASTSET) the (local) Velocity
208 //requested by LSL 211 //requested by LSL
209 private float m_linearMotorTimescale = 0; // Motor Attack rate set by LSL 212 private float m_linearMotorTimescale = 0; // Motor Attack rate set by LSL
210 private float m_linearMotorDecayTimescale = 0; // Motor Decay rate set by LSL 213 private float m_linearMotorDecayTimescale = 0; // Motor Decay rate set by LSL
211 private Vector3 m_linearFrictionTimescale = Vector3.Zero; // General Friction set by LSL 214 private Vector3 m_linearFrictionTimescale = Vector3.Zero; // General Friction set by LSL
212 215
213 private Vector3 m_lLinMotorDVel = Vector3.Zero; // decayed motor 216 private Vector3 m_lLinMotorDVel = Vector3.Zero; // decayed motor
214 private Vector3 m_lLinObjectVel = Vector3.Zero; // local frame object velocity 217 private Vector3 m_lLinObjectVel = Vector3.Zero; // local frame object velocity
215 private Vector3 m_wLinObjectVel = Vector3.Zero; // world frame object velocity 218 private Vector3 m_wLinObjectVel = Vector3.Zero; // world frame object velocity
216 219
217 //Angular properties 220 //Angular properties
218 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor 221 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
219 222
220 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL 223 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
221 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL 224 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
222 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL 225 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
223 226
224 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor 227 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
225// private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity 228 // private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity
226 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body 229 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
227 230
228 //Deflection properties 231 //Deflection properties
229 // private float m_angularDeflectionEfficiency = 0; 232 // private float m_angularDeflectionEfficiency = 0;
230 // private float m_angularDeflectionTimescale = 0; 233 // private float m_angularDeflectionTimescale = 0;
231 // private float m_linearDeflectionEfficiency = 0; 234 // private float m_linearDeflectionEfficiency = 0;
232 // private float m_linearDeflectionTimescale = 0; 235 // private float m_linearDeflectionTimescale = 0;
233 236
234 //Banking properties 237 //Banking properties
235 // private float m_bankingEfficiency = 0; 238 // private float m_bankingEfficiency = 0;
236 // private float m_bankingMix = 0; 239 // private float m_bankingMix = 0;
237 // private float m_bankingTimescale = 0; 240 // private float m_bankingTimescale = 0;
238 241
239 //Hover and Buoyancy properties 242 //Hover and Buoyancy properties
240 private float m_VhoverHeight = 0f; 243 private float m_VhoverHeight = 0f;
241// private float m_VhoverEfficiency = 0f; 244 // private float m_VhoverEfficiency = 0f;
242 private float m_VhoverTimescale = 0f; 245 private float m_VhoverTimescale = 0f;
243 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height 246 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
244 private float m_VehicleBuoyancy = 0f; // Set by VEHICLE_BUOYANCY, for a vehicle. 247 private float m_VehicleBuoyancy = 0f; // Set by VEHICLE_BUOYANCY, for a vehicle.
245 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) 248 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
246 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. 249 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
247 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. 250 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
248 251
249 //Attractor properties 252 //Attractor properties
250 private float m_verticalAttractionEfficiency = 1.0f; // damped 253 private float m_verticalAttractionEfficiency = 1.0f; // damped
251 private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor. 254 private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor.
252 255
253 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;
254 302
255 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);
256 397
398 vtmp.X = dvt.X;
399 vtmp.Y = dvt.Y;
400 vtmp.Z = dvt.Z;
401
402 d.Quaternion dqt;
403 d.GeomCopyQuaternion(prim_geom, out dqt);
404
405 to.X = dqt.X;
406 to.Y = dqt.Y;
407 to.Z = dqt.Z;
408 to.W = dqt.W; // rotation in world
409 }
410 else
411 {
412 vtmp = _position;
413 to = _orientation;
414 }
415
416 vtmp -= _parent.Position; // offset in world
417 vtmp *= ipo; // offset in local
418 st.Wvector3(vtmp);
419
420 ipo *= to; // own rotation
421 st.Wquat(ipo);
422 }
423
424 st.Wvector3(_velocity);
425 st.Wvector3(m_rotationalVelocity);
426 st.Wvector3(_acceleration);
427 st.Wvector3(m_rotateEnable);
428
429 vtmp = Vector3.Zero;
430 for (int i = 0; i < m_forcelist.Count; i++)
431 {
432
433 vtmp += (m_forcelist[i] * 100);
434 }
435
436 st.Wvector3(vtmp); // force acc
437
438 vtmp = Vector3.Zero;
439 for (int i = 0; i < m_angularforcelist.Count; i++)
440 {
441 vtmp += (m_angularforcelist[i] * 100);
442 }
443
444 st.Wvector3(vtmp); // angular force acc
445
446 st.Wvector3(m_PIDTarget);
447 st.Wfloat(m_PIDTau);
448 st.Wfloat(PID_D);
449 st.Wfloat(PID_G);
450 st.Wquat(m_APIDTarget);
451 st.Wfloat(m_APIDStrength);
452 st.Wfloat(m_APIDDamping);
453 st.Wfloat(m_APIDdamper);
454
455 st.Wint((int)m_PIDHoverType);
456 st.Wfloat(m_PIDHoverHeight);
457 st.Wfloat(m_PIDHoverTau);
458 st.Wfloat(m_targetHoverHeight);
459
460 st.Wfloat(m_groundHeight);
461 st.Wfloat(m_waterHeight);
462
463 st.Wfloat(m_buoyancy);
464
465 // this must be last since type none ends stream
466 if (m_type == Vehicle.TYPE_NONE)
467 st.Wint((int)Vehicle.TYPE_NONE);
468 else
469 {
470 st.Wint((int)m_type);
471
472 st.Wquat(Quaternion.Identity); //m_referenceFrame
473
474 st.Wint((int)m_flags);
475
476 st.Wvector3(m_linearMotorDirection);
477 st.Wfloat(
478 (float)Math.Sqrt(m_lLinMotorDVel.LengthSquared() / m_linearMotorDirection.LengthSquared()));
479
480 st.Wvector3(m_linearFrictionTimescale);
481 st.Wfloat(m_linearMotorDecayTimescale);
482 st.Wfloat(m_linearMotorTimescale);
483 st.Wvector3(new Vector3(0, 0, 0)); //m_linearMotorOffset);
484
485 st.Wvector3(m_angularMotorDirection);
486 st.Wfloat((float)Math.Sqrt(m_angularMotorDVel.LengthSquared() / m_angularMotorDirection.LengthSquared()));
487
488 st.Wvector3(m_angularFrictionTimescale);
489 st.Wfloat(m_angularMotorDecayTimescale);
490 st.Wfloat(m_angularMotorTimescale);
491
492 st.Wfloat(0); //m_linearDeflectionEfficiency);
493 st.Wfloat(1000); //m_linearDeflectionTimescale);
494
495 st.Wfloat(0); //m_angularDeflectionEfficiency);
496 st.Wfloat(120); //m_angularDeflectionTimescale);
497
498 st.Wfloat(0); // m_bankingEfficiency);
499 st.Wfloat(0); //m_bankingMix);
500 st.Wfloat(1000); //m_bankingTimescale);
501
502 st.Wfloat(m_VhoverHeight);
503 st.Wfloat(0.5f); //m_VhoverEfficiency);
504 st.Wfloat(m_VhoverTimescale);
505
506 st.Wfloat(m_VehicleBuoyancy);
507
508 st.Wfloat(m_verticalAttractionEfficiency);
509 st.Wfloat(m_verticalAttractionTimescale);
510 }
511 sc.data = st.close();
512 m_taintserial = null;
513 Monitor.PulseAll(sc.alock);
514 }
515
516 public bool DeSerialize(byte[] data)
517 {
518 rstreamer st = new rstreamer(data);
519
520 int version =st.Rushort(); //version
521
522 // merge booleans in a ushort
523 ushort flags = st.Rushort();
524 if ((flags & 1) != 0)
525 m_isphysical = true;
526 if ((flags & 2) != 0)
527 m_taintselected = true;
528 if ((flags & 4) != 0)
529 m_isVolumeDetect = true;
530 if ((flags & 8) != 0)
531 m_taintdisable = true;
532 if ((flags & 16) != 0)
533 m_taintCollidesWater = true;
534 if ((flags & 32) != 0)
535 m_collidesLand = true;
536 if ((flags & 64) != 0)
537 m_usePID = true;
538 if ((flags & 128) != 0)
539 m_useAPID = true;
540 if ((flags & 256) != 0)
541 m_useHoverPID = true;
542 if ((flags & 512) != 0)
543 m_throttleUpdates = true;
544
545 _size = st.Rvector3();
546 m_taintsize = _size;
547
548 m_material= st.Rint();
549 m_density = st.Rfloat();
550 st.Rfloat(); // future gravity mod V3
551 st.Rfloat(); // future friction V3
552 st.Rfloat(); // future bounce V3
553
554// m_collisionCategories = (CollisionCategories)st.Ruint();
555// m_collisionFlags = (CollisionCategories) st.Ruint();
556
557 if (m_taintparent == null)
558 {
559 st.Rvector3(); // ignore old position sop/sog as to tell the new one
560 m_taintrot = st.Rquat(); //
561 _orientation = m_taintrot;
562 }
563 else
564 {
565 m_taintrot = _parent.Orientation;
566 m_taintposition = st.Rvector3(); // ??
567 _position = m_taintposition;
568
569 m_taintposition *= m_taintrot;
570 m_taintposition += _parent.Position;
571
572 m_taintrot *= st.Rquat(); //
573 _orientation = m_taintrot;
574 }
575
576 m_taintVelocity = st.Rvector3();
577 m_rotationalVelocity = st.Rvector3();
578
579 _acceleration = st.Rvector3();
580 m_rotateEnableRequest = st.Rvector3();
581 m_rotateEnableUpdate = true;
582
583 Vector3 vtmp;
584
585 vtmp = st.Rvector3(); // forces acc
586 m_forcelist.Add(vtmp);
587 m_taintforce = true;
588
589 vtmp = st.Rvector3(); // angular forces acc
590 m_angularforcelist.Add(vtmp);
591 m_taintaddangularforce = true;
592
593 m_PIDTarget = st.Rvector3();
594 m_PIDTau = st.Rfloat();
595 PID_D = st.Rfloat();
596 PID_G = st.Rfloat();
597
598 m_APIDTarget = st.Rquat();
599 m_APIDStrength = st.Rfloat();
600 m_APIDDamping = st.Rfloat();
601 m_APIDdamper = st.Rfloat();
602
603 m_PIDHoverType = (PIDHoverType) st.Rint();
604 m_PIDHoverHeight = st.Rfloat();
605 m_PIDHoverTau = st.Rfloat();
606 m_targetHoverHeight = st.Rfloat();
607
608 m_groundHeight = st.Rfloat();
609 m_waterHeight = st.Rfloat();
610
611 m_buoyancy = st.Rfloat();
612
613
614 // this must be last since type none ends stream
615
616 m_type = (Vehicle) st.Rint();
617
618 if (m_type != Vehicle.TYPE_NONE)
619 {
620 float ftmp;
621
622 st.Rquat(); //m_referenceFrame
623
624 m_flags = (VehicleFlag) st.Rint();
625
626 m_linearMotorDirection = st.Rvector3();
627
628 ftmp = st.Rfloat();
629 m_lLinMotorDVel = m_linearMotorDirection * ftmp;
630
631 m_linearFrictionTimescale = st.Rvector3();
632 m_linearMotorDecayTimescale = st.Rfloat();
633 m_linearMotorTimescale = st.Rfloat();
634 st.Rvector3(); //m_linearMotorOffset);
635
636 m_angularMotorDirection = st.Rvector3();
637 ftmp = st.Rfloat();
638 m_angularMotorDVel = m_angularMotorDirection * ftmp;
639
640 m_angularFrictionTimescale = st.Rvector3();
641 m_angularMotorDecayTimescale = st.Rfloat();
642 m_angularMotorTimescale = st.Rfloat();
643
644 st.Rfloat(); //m_linearDeflectionEfficiency);
645 st.Rfloat(); //m_linearDeflectionTimescale);
646
647 st.Rfloat(); //m_angularDeflectionEfficiency);
648 st.Rfloat(); //m_angularDeflectionTimescale);
649
650 st.Rfloat(); // m_bankingEfficiency);
651 st.Rfloat(); //m_bankingMix);
652 st.Rfloat(); //m_bankingTimescale);
653
654 m_VhoverHeight = st.Rfloat();
655 st.Rfloat(); //m_VhoverEfficiency);
656 m_VhoverTimescale = st.Rfloat();
657
658 m_VehicleBuoyancy = st.Rfloat();
659
660 m_verticalAttractionEfficiency = st.Rfloat();
661 m_verticalAttractionTimescale = st.Rfloat();
662 }
663 st.close();
664 return true;
665 }
666
667 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, PhysicsActor parent,
668 PrimitiveBaseShape pbs, CollisionLocker dode, uint localid, byte[] sdata)
669 {
670 m_localID = localid;
671 ode = dode;
672
673 if (parent == null)
674 {
675 m_taintparent = null;
676
677 if (!pos.IsFinite())
678 {
679 pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
680 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
681 m_log.Warn("[PHYSICS]: Got nonFinite Object create Position");
682 }
683
684 _position = pos;
685 m_taintposition = pos;
686 }
687 else
688 m_taintparent = parent;
689
690 body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
691
692 prim_geom = IntPtr.Zero;
693
694 _mesh = null;
695 m_meshfailed = false;
696 _pbs = pbs;
697
698 _parent_scene = parent_scene;
699 m_targetSpace = (IntPtr)0;
700
701 if(sdata != null && sdata.Length > 1)
702 DeSerialize(sdata);
703
704 if (m_isphysical)
705 m_targetSpace = _parent_scene.space;
706
707 m_primName = primName;
708 m_taintserial = null;
709 m_taintadd = true;
710 _parent_scene.AddPhysicsActorTaint(this);
711 // don't do .add() here; old geoms get recycled with the same hash
712 }
257 713
258 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, 714 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
259 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode, uint localid) 715 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode, uint localid)
@@ -266,7 +722,7 @@ namespace OpenSim.Region.Physics.OdePlugin
266 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); 722 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
267 m_log.Warn("[PHYSICS]: Got nonFinite Object create Position"); 723 m_log.Warn("[PHYSICS]: Got nonFinite Object create Position");
268 } 724 }
269 725
270 _position = pos; 726 _position = pos;
271 m_taintposition = pos; 727 m_taintposition = pos;
272 PID_D = parent_scene.bodyPIDD; 728 PID_D = parent_scene.bodyPIDD;
@@ -275,9 +731,8 @@ namespace OpenSim.Region.Physics.OdePlugin
275 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; 731 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
276 body_autodisable_frames = parent_scene.bodyFramesAutoDisable; 732 body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
277 733
278
279 prim_geom = IntPtr.Zero; 734 prim_geom = IntPtr.Zero;
280// prev_geom = IntPtr.Zero; 735 // prev_geom = IntPtr.Zero;
281 736
282 if (!pos.IsFinite()) 737 if (!pos.IsFinite())
283 { 738 {
@@ -306,7 +761,7 @@ namespace OpenSim.Region.Physics.OdePlugin
306 _parent_scene = parent_scene; 761 _parent_scene = parent_scene;
307 m_targetSpace = (IntPtr)0; 762 m_targetSpace = (IntPtr)0;
308 763
309// if (pos.Z < 0) 764 // if (pos.Z < 0)
310 if (pos.Z < parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y)) 765 if (pos.Z < parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y))
311 m_isphysical = false; 766 m_isphysical = false;
312 else 767 else
@@ -317,6 +772,8 @@ namespace OpenSim.Region.Physics.OdePlugin
317 if (m_isphysical) 772 if (m_isphysical)
318 m_targetSpace = _parent_scene.space; 773 m_targetSpace = _parent_scene.space;
319 } 774 }
775
776 m_taintserial = null;
320 m_primName = primName; 777 m_primName = primName;
321 m_taintadd = true; 778 m_taintadd = true;
322 _parent_scene.AddPhysicsActorTaint(this); 779 _parent_scene.AddPhysicsActorTaint(this);
@@ -325,7 +782,7 @@ namespace OpenSim.Region.Physics.OdePlugin
325 782
326 public override int PhysicsActorType 783 public override int PhysicsActorType
327 { 784 {
328 get { return (int) ActorTypes.Prim; } 785 get { return (int)ActorTypes.Prim; }
329 set { return; } 786 set { return; }
330 } 787 }
331 788
@@ -337,9 +794,11 @@ namespace OpenSim.Region.Physics.OdePlugin
337 794
338 public override uint LocalID 795 public override uint LocalID
339 { 796 {
340 set { 797 set
798 {
341 //m_log.Info("[PHYSICS]: Setting TrackerID: " + value); 799 //m_log.Info("[PHYSICS]: Setting TrackerID: " + value);
342 m_localID = value; } 800 m_localID = value;
801 }
343 } 802 }
344 803
345 public override bool Grabbed 804 public override bool Grabbed
@@ -349,9 +808,10 @@ namespace OpenSim.Region.Physics.OdePlugin
349 808
350 public override bool Selected 809 public override bool Selected
351 { 810 {
352 set { 811 set
353 812 {
354//Console.WriteLine("Sel {0} {1} {2}", m_primName, value, m_isphysical); 813
814 //Console.WriteLine("Sel {0} {1} {2}", m_primName, value, m_isphysical);
355 // This only makes the object not collidable if the object 815 // This only makes the object not collidable if the object
356 // is physical or the object is modified somehow *IN THE FUTURE* 816 // is physical or the object is modified somehow *IN THE FUTURE*
357 // without this, if an avatar selects prim, they can walk right 817 // without this, if an avatar selects prim, they can walk right
@@ -367,21 +827,21 @@ namespace OpenSim.Region.Physics.OdePlugin
367 m_taintselected = value; 827 m_taintselected = value;
368 m_isSelected = value; 828 m_isSelected = value;
369 } 829 }
370 if(m_isSelected) disableBodySoft(); 830 if (m_isSelected) disableBodySoft();
371 } 831 }
372 } 832 }
373 833
374 public override bool IsPhysical 834 public override bool IsPhysical
375 { 835 {
376 get { return m_isphysical; } 836 get { return m_isphysical; }
377 set 837 set
378 { 838 {
379 m_isphysical = value; 839 m_isphysical = value;
380 if (!m_isphysical) 840 if (!m_isphysical)
381 { // Zero the remembered last velocity 841 { // Zero the remembered last velocity
382 m_lastVelocity = Vector3.Zero; 842 m_lastVelocity = Vector3.Zero;
383 if (m_type != Vehicle.TYPE_NONE) Halt(); 843 if (m_type != Vehicle.TYPE_NONE) Halt();
384 } 844 }
385 } 845 }
386 } 846 }
387 847
@@ -430,7 +890,9 @@ namespace OpenSim.Region.Physics.OdePlugin
430 { 890 {
431 get { return _position; } 891 get { return _position; }
432 892
433 set { _position = value; 893 set
894 {
895 _position = value;
434 //m_log.Info("[PHYSICS]: " + _position.ToString()); 896 //m_log.Info("[PHYSICS]: " + _position.ToString());
435 } 897 }
436 } 898 }
@@ -481,29 +943,29 @@ namespace OpenSim.Region.Physics.OdePlugin
481 943
482 public override void VehicleFloatParam(int param, float value) 944 public override void VehicleFloatParam(int param, float value)
483 { 945 {
484 ProcessFloatVehicleParam((Vehicle) param, value); 946 ProcessFloatVehicleParam((Vehicle)param, value);
485 } 947 }
486 948
487 public override void VehicleVectorParam(int param, Vector3 value) 949 public override void VehicleVectorParam(int param, Vector3 value)
488 { 950 {
489 ProcessVectorVehicleParam((Vehicle) param, value); 951 ProcessVectorVehicleParam((Vehicle)param, value);
490 } 952 }
491 953
492 public override void VehicleRotationParam(int param, Quaternion rotation) 954 public override void VehicleRotationParam(int param, Quaternion rotation)
493 { 955 {
494 ProcessRotationVehicleParam((Vehicle) param, rotation); 956 ProcessRotationVehicleParam((Vehicle)param, rotation);
495 } 957 }
496 958
497 public override void VehicleFlags(int param, bool remove) 959 public override void VehicleFlags(int param, bool remove)
498 { 960 {
499 ProcessVehicleFlags(param, remove); 961 ProcessVehicleFlags(param, remove);
500 } 962 }
501 963
502 public override void SetVolumeDetect(int param) 964 public override void SetVolumeDetect(int param)
503 { 965 {
504 lock (_parent_scene.OdeLock) 966 lock (_parent_scene.OdeLock)
505 { 967 {
506 m_isVolumeDetect = (param!=0); 968 m_isVolumeDetect = (param != 0);
507 } 969 }
508 } 970 }
509 971
@@ -536,9 +998,9 @@ namespace OpenSim.Region.Physics.OdePlugin
536 return Vector3.Zero; 998 return Vector3.Zero;
537 999
538 Vector3 returnVelocity = Vector3.Zero; 1000 Vector3 returnVelocity = Vector3.Zero;
539 returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2; 1001 returnVelocity.X = (m_lastVelocity.X + _velocity.X) / 2;
540 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2; 1002 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) / 2;
541 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2; 1003 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) / 2;
542 return returnVelocity; 1004 return returnVelocity;
543 } 1005 }
544 set 1006 set
@@ -546,8 +1008,8 @@ namespace OpenSim.Region.Physics.OdePlugin
546 if (value.IsFinite()) 1008 if (value.IsFinite())
547 { 1009 {
548 _velocity = value; 1010 _velocity = value;
549 if (_velocity.ApproxEquals(Vector3.Zero,0.001f)) 1011 if (_velocity.ApproxEquals(Vector3.Zero, 0.001f))
550 _acceleration = Vector3.Zero; 1012 _acceleration = Vector3.Zero;
551 1013
552 m_taintVelocity = value; 1014 m_taintVelocity = value;
553 _parent_scene.AddPhysicsActorTaint(this); 1015 _parent_scene.AddPhysicsActorTaint(this);
@@ -604,17 +1066,18 @@ namespace OpenSim.Region.Physics.OdePlugin
604 if (QuaternionIsFinite(value)) 1066 if (QuaternionIsFinite(value))
605 { 1067 {
606 _orientation = value; 1068 _orientation = value;
607 } 1069 }
608 else 1070 else
609 m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object"); 1071 m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object");
610 1072
611 } 1073 }
612 } 1074 }
613 1075
614 1076
615 public override bool FloatOnWater 1077 public override bool FloatOnWater
616 { 1078 {
617 set { 1079 set
1080 {
618 m_taintCollidesWater = value; 1081 m_taintCollidesWater = value;
619 _parent_scene.AddPhysicsActorTaint(this); 1082 _parent_scene.AddPhysicsActorTaint(this);
620 } 1083 }
@@ -624,8 +1087,8 @@ namespace OpenSim.Region.Physics.OdePlugin
624 { 1087 {
625 } 1088 }
626 1089
627 public override Vector3 PIDTarget 1090 public override Vector3 PIDTarget
628 { 1091 {
629 set 1092 set
630 { 1093 {
631 if (value.IsFinite()) 1094 if (value.IsFinite())
@@ -634,16 +1097,16 @@ namespace OpenSim.Region.Physics.OdePlugin
634 } 1097 }
635 else 1098 else
636 m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object"); 1099 m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object");
637 } 1100 }
638 } 1101 }
639 public override bool PIDActive { set { m_usePID = value; } } 1102 public override bool PIDActive { set { m_usePID = value; } }
640 public override float PIDTau { set { m_PIDTau = value; } } 1103 public override float PIDTau { set { m_PIDTau = value; } }
641 1104
642 // For RotLookAt 1105 // For RotLookAt
643 public override Quaternion APIDTarget { set { m_APIDTarget = value; } } 1106 public override Quaternion APIDTarget { set { m_APIDTarget = value; } }
644 public override bool APIDActive { set { m_useAPID = value; } } 1107 public override bool APIDActive { set { m_useAPID = value; } }
645 public override float APIDStrength { set { m_APIDStrength = value; } } 1108 public override float APIDStrength { set { m_APIDStrength = value; } }
646 public override float APIDDamping { set { m_APIDDamping = value; } } 1109 public override float APIDDamping { set { m_APIDDamping = value; } }
647 1110
648 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } 1111 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
649 public override bool PIDHoverActive { set { m_useHoverPID = value; } } 1112 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
@@ -665,13 +1128,13 @@ namespace OpenSim.Region.Physics.OdePlugin
665 1128
666 public override Vector3 Acceleration // client updates read data via here 1129 public override Vector3 Acceleration // client updates read data via here
667 { 1130 {
668 get 1131 get
669 { 1132 {
670 if (_zeroFlag) 1133 if (_zeroFlag)
671 { 1134 {
672 return Vector3.Zero; 1135 return Vector3.Zero;
673 } 1136 }
674 return _acceleration; 1137 return _acceleration;
675 } 1138 }
676 set { _acceleration = value; } 1139 set { _acceleration = value; }
677 } 1140 }
@@ -752,18 +1215,18 @@ namespace OpenSim.Region.Physics.OdePlugin
752 base.RequestPhysicsterseUpdate(); 1215 base.RequestPhysicsterseUpdate();
753 m_outofBounds = false; 1216 m_outofBounds = false;
754 } 1217 }
755/* 1218 /*
756 int tmp = Interlocked.Increment(ref m_crossingfailures); 1219 int tmp = Interlocked.Increment(ref m_crossingfailures);
757 if (tmp > _parent_scene.geomCrossingFailuresBeforeOutofbounds) 1220 if (tmp > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
758 { 1221 {
759 base.RaiseOutOfBounds(_position); 1222 base.RaiseOutOfBounds(_position);
760 return; 1223 return;
761 } 1224 }
762 else if (tmp == _parent_scene.geomCrossingFailuresBeforeOutofbounds) 1225 else if (tmp == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
763 { 1226 {
764 m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName); 1227 m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);
765 } 1228 }
766 */ 1229 */
767 } 1230 }
768 1231
769 public override float Buoyancy 1232 public override float Buoyancy
@@ -806,26 +1269,26 @@ namespace OpenSim.Region.Physics.OdePlugin
806 1269
807 public void SetGeom(IntPtr geom) 1270 public void SetGeom(IntPtr geom)
808 { 1271 {
809 if(prim_geom != IntPtr.Zero) 1272 if (prim_geom != IntPtr.Zero)
810 { 1273 {
811 // Remove any old entries 1274 // Remove any old entries
812//string tPA; 1275 //string tPA;
813//_parent_scene.geom_name_map.TryGetValue(prim_geom, out tPA); 1276 //_parent_scene.geom_name_map.TryGetValue(prim_geom, out tPA);
814//Console.WriteLine("**** Remove {0}", tPA); 1277 //Console.WriteLine("**** Remove {0}", tPA);
815 if(_parent_scene.geom_name_map.ContainsKey(prim_geom)) _parent_scene.geom_name_map.Remove(prim_geom); 1278 if (_parent_scene.geom_name_map.ContainsKey(prim_geom)) _parent_scene.geom_name_map.Remove(prim_geom);
816 if(_parent_scene.actor_name_map.ContainsKey(prim_geom)) _parent_scene.actor_name_map.Remove(prim_geom); 1279 if (_parent_scene.actor_name_map.ContainsKey(prim_geom)) _parent_scene.actor_name_map.Remove(prim_geom);
817 d.GeomDestroy(prim_geom); 1280 d.GeomDestroy(prim_geom);
818 } 1281 }
819 1282
820 prim_geom = geom; 1283 prim_geom = geom;
821//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName); 1284 //Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName);
822 if (prim_geom != IntPtr.Zero) 1285 if (prim_geom != IntPtr.Zero)
823 { 1286 {
824 _parent_scene.geom_name_map[prim_geom] = this.m_primName; 1287 _parent_scene.geom_name_map[prim_geom] = this.m_primName;
825 _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this; 1288 _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
826 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 1289 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
827 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 1290 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
828//Console.WriteLine("**** Create {2} Dicts: actor={0} name={1}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, this.m_primName); 1291 //Console.WriteLine("**** Create {2} Dicts: actor={0} name={1}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, this.m_primName);
829 } 1292 }
830 1293
831 if (childPrim) 1294 if (childPrim)
@@ -833,7 +1296,7 @@ namespace OpenSim.Region.Physics.OdePlugin
833 if (_parent != null && _parent is OdePrim) 1296 if (_parent != null && _parent is OdePrim)
834 { 1297 {
835 OdePrim parent = (OdePrim)_parent; 1298 OdePrim parent = (OdePrim)_parent;
836//Console.WriteLine("SetGeom calls ChildSetGeom"); 1299 //Console.WriteLine("SetGeom calls ChildSetGeom");
837 parent.ChildSetGeom(this); 1300 parent.ChildSetGeom(this);
838 } 1301 }
839 } 1302 }
@@ -848,7 +1311,7 @@ namespace OpenSim.Region.Physics.OdePlugin
848 { 1311 {
849 d.BodyEnable(Body); 1312 d.BodyEnable(Body);
850 if (m_type != Vehicle.TYPE_NONE) 1313 if (m_type != Vehicle.TYPE_NONE)
851 Enable(Body, _parent_scene); 1314 Enable(Body, _parent_scene);
852 } 1315 }
853 1316
854 m_disabled = false; 1317 m_disabled = false;
@@ -892,9 +1355,9 @@ namespace OpenSim.Region.Physics.OdePlugin
892 1355
893 d.BodySetAutoDisableFlag(Body, true); 1356 d.BodySetAutoDisableFlag(Body, true);
894 d.BodySetAutoDisableSteps(Body, body_autodisable_frames); 1357 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
895 1358
896 // disconnect from world gravity so we can apply buoyancy 1359 // disconnect from world gravity so we can apply buoyancy
897 d.BodySetGravityMode (Body, false); 1360 d.BodySetGravityMode(Body, false);
898 1361
899 m_interpenetrationcount = 0; 1362 m_interpenetrationcount = 0;
900 m_collisionscore = 0; 1363 m_collisionscore = 0;
@@ -918,19 +1381,19 @@ namespace OpenSim.Region.Physics.OdePlugin
918 1381
919 float returnMass = 0; 1382 float returnMass = 0;
920 float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; 1383 float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
921 float hollowVolume = hollowAmount * hollowAmount; 1384 float hollowVolume = hollowAmount * hollowAmount;
922 1385
923 switch (_pbs.ProfileShape) 1386 switch (_pbs.ProfileShape)
924 { 1387 {
925 case ProfileShape.Square: 1388 case ProfileShape.Square:
926 // default box 1389 // default box
927 1390
928 if (_pbs.PathCurve == (byte)Extrusion.Straight) 1391 if (_pbs.PathCurve == (byte)Extrusion.Straight)
929 { 1392 {
930 if (hollowAmount > 0.0) 1393 if (hollowAmount > 0.0)
931 { 1394 {
932 switch (_pbs.HollowShape) 1395 switch (_pbs.HollowShape)
933 { 1396 {
934 case HollowShape.Square: 1397 case HollowShape.Square:
935 case HollowShape.Same: 1398 case HollowShape.Same:
936 break; 1399 break;
@@ -948,31 +1411,31 @@ namespace OpenSim.Region.Physics.OdePlugin
948 default: 1411 default:
949 hollowVolume = 0; 1412 hollowVolume = 0;
950 break; 1413 break;
951 }
952 volume *= (1.0f - hollowVolume);
953 } 1414 }
1415 volume *= (1.0f - hollowVolume);
954 } 1416 }
1417 }
955 1418
956 else if (_pbs.PathCurve == (byte)Extrusion.Curve1) 1419 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
957 { 1420 {
958 //a tube 1421 //a tube
959 1422
960 volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); 1423 volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
961 tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY); 1424 tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY);
962 volume -= volume*tmp*tmp; 1425 volume -= volume * tmp * tmp;
963 1426
964 if (hollowAmount > 0.0) 1427 if (hollowAmount > 0.0)
965 { 1428 {
966 hollowVolume *= hollowAmount; 1429 hollowVolume *= hollowAmount;
967 1430
968 switch (_pbs.HollowShape) 1431 switch (_pbs.HollowShape)
969 { 1432 {
970 case HollowShape.Square: 1433 case HollowShape.Square:
971 case HollowShape.Same: 1434 case HollowShape.Same:
972 break; 1435 break;
973 1436
974 case HollowShape.Circle: 1437 case HollowShape.Circle:
975 hollowVolume *= 0.78539816339f;; 1438 hollowVolume *= 0.78539816339f; ;
976 break; 1439 break;
977 1440
978 case HollowShape.Triangle: 1441 case HollowShape.Triangle:
@@ -981,23 +1444,23 @@ namespace OpenSim.Region.Physics.OdePlugin
981 default: 1444 default:
982 hollowVolume = 0; 1445 hollowVolume = 0;
983 break; 1446 break;
984 }
985 volume *= (1.0f - hollowVolume);
986 } 1447 }
1448 volume *= (1.0f - hollowVolume);
987 } 1449 }
1450 }
988 1451
989 break; 1452 break;
990 1453
991 case ProfileShape.Circle: 1454 case ProfileShape.Circle:
992 1455
993 if (_pbs.PathCurve == (byte)Extrusion.Straight) 1456 if (_pbs.PathCurve == (byte)Extrusion.Straight)
994 { 1457 {
995 volume *= 0.78539816339f; // elipse base 1458 volume *= 0.78539816339f; // elipse base
996 1459
997 if (hollowAmount > 0.0) 1460 if (hollowAmount > 0.0)
998 { 1461 {
999 switch (_pbs.HollowShape) 1462 switch (_pbs.HollowShape)
1000 { 1463 {
1001 case HollowShape.Same: 1464 case HollowShape.Same:
1002 case HollowShape.Circle: 1465 case HollowShape.Circle:
1003 break; 1466 break;
@@ -1013,25 +1476,25 @@ namespace OpenSim.Region.Physics.OdePlugin
1013 default: 1476 default:
1014 hollowVolume = 0; 1477 hollowVolume = 0;
1015 break; 1478 break;
1016 }
1017 volume *= (1.0f - hollowVolume);
1018 } 1479 }
1480 volume *= (1.0f - hollowVolume);
1019 } 1481 }
1482 }
1020 1483
1021 else if (_pbs.PathCurve == (byte)Extrusion.Curve1) 1484 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1022 { 1485 {
1023 volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); 1486 volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
1024 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); 1487 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
1025 volume *= (1.0f - tmp * tmp); 1488 volume *= (1.0f - tmp * tmp);
1026 1489
1027 if (hollowAmount > 0.0) 1490 if (hollowAmount > 0.0)
1028 { 1491 {
1029 1492
1030 // calculate the hollow volume by it's shape compared to the prim shape 1493 // calculate the hollow volume by it's shape compared to the prim shape
1031 hollowVolume *= hollowAmount; 1494 hollowVolume *= hollowAmount;
1032 1495
1033 switch (_pbs.HollowShape) 1496 switch (_pbs.HollowShape)
1034 { 1497 {
1035 case HollowShape.Same: 1498 case HollowShape.Same:
1036 case HollowShape.Circle: 1499 case HollowShape.Circle:
1037 break; 1500 break;
@@ -1047,31 +1510,31 @@ namespace OpenSim.Region.Physics.OdePlugin
1047 default: 1510 default:
1048 hollowVolume = 0; 1511 hollowVolume = 0;
1049 break; 1512 break;
1050 }
1051 volume *= (1.0f - hollowVolume);
1052 } 1513 }
1514 volume *= (1.0f - hollowVolume);
1053 } 1515 }
1516 }
1054 break; 1517 break;
1055 1518
1056 case ProfileShape.HalfCircle: 1519 case ProfileShape.HalfCircle:
1057 if (_pbs.PathCurve == (byte)Extrusion.Curve1) 1520 if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1058 { 1521 {
1059 volume *= 0.52359877559829887307710723054658f; 1522 volume *= 0.52359877559829887307710723054658f;
1060 } 1523 }
1061 break; 1524 break;
1062 1525
1063 case ProfileShape.EquilateralTriangle: 1526 case ProfileShape.EquilateralTriangle:
1064 1527
1065 if (_pbs.PathCurve == (byte)Extrusion.Straight) 1528 if (_pbs.PathCurve == (byte)Extrusion.Straight)
1066 { 1529 {
1067 volume *= 0.32475953f; 1530 volume *= 0.32475953f;
1068 1531
1069 if (hollowAmount > 0.0) 1532 if (hollowAmount > 0.0)
1070 { 1533 {
1071 1534
1072 // calculate the hollow volume by it's shape compared to the prim shape 1535 // calculate the hollow volume by it's shape compared to the prim shape
1073 switch (_pbs.HollowShape) 1536 switch (_pbs.HollowShape)
1074 { 1537 {
1075 case HollowShape.Same: 1538 case HollowShape.Same:
1076 case HollowShape.Triangle: 1539 case HollowShape.Triangle:
1077 hollowVolume *= .25f; 1540 hollowVolume *= .25f;
@@ -1091,24 +1554,24 @@ namespace OpenSim.Region.Physics.OdePlugin
1091 default: 1554 default:
1092 hollowVolume = 0; 1555 hollowVolume = 0;
1093 break; 1556 break;
1094 }
1095 volume *= (1.0f - hollowVolume);
1096 } 1557 }
1558 volume *= (1.0f - hollowVolume);
1097 } 1559 }
1560 }
1098 else if (_pbs.PathCurve == (byte)Extrusion.Curve1) 1561 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1099 { 1562 {
1100 volume *= 0.32475953f; 1563 volume *= 0.32475953f;
1101 volume *= 0.01f * (float)(200 - _pbs.PathScaleX); 1564 volume *= 0.01f * (float)(200 - _pbs.PathScaleX);
1102 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); 1565 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
1103 volume *= (1.0f - tmp * tmp); 1566 volume *= (1.0f - tmp * tmp);
1104 1567
1105 if (hollowAmount > 0.0) 1568 if (hollowAmount > 0.0)
1106 { 1569 {
1107 1570
1108 hollowVolume *= hollowAmount; 1571 hollowVolume *= hollowAmount;
1109 1572
1110 switch (_pbs.HollowShape) 1573 switch (_pbs.HollowShape)
1111 { 1574 {
1112 case HollowShape.Same: 1575 case HollowShape.Same:
1113 case HollowShape.Triangle: 1576 case HollowShape.Triangle:
1114 hollowVolume *= .25f; 1577 hollowVolume *= .25f;
@@ -1126,15 +1589,15 @@ namespace OpenSim.Region.Physics.OdePlugin
1126 default: 1589 default:
1127 hollowVolume = 0; 1590 hollowVolume = 0;
1128 break; 1591 break;
1129 }
1130 volume *= (1.0f - hollowVolume);
1131 } 1592 }
1593 volume *= (1.0f - hollowVolume);
1132 } 1594 }
1133 break; 1595 }
1596 break;
1134 1597
1135 default: 1598 default:
1136 break; 1599 break;
1137 } 1600 }
1138 1601
1139 1602
1140 1603
@@ -1148,7 +1611,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1148 float profileEnd; 1611 float profileEnd;
1149 1612
1150 if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) 1613 if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible)
1151 { 1614 {
1152 taperX1 = _pbs.PathScaleX * 0.01f; 1615 taperX1 = _pbs.PathScaleX * 0.01f;
1153 if (taperX1 > 1.0f) 1616 if (taperX1 > 1.0f)
1154 taperX1 = 2.0f - taperX1; 1617 taperX1 = 2.0f - taperX1;
@@ -1158,9 +1621,9 @@ namespace OpenSim.Region.Physics.OdePlugin
1158 if (taperY1 > 1.0f) 1621 if (taperY1 > 1.0f)
1159 taperY1 = 2.0f - taperY1; 1622 taperY1 = 2.0f - taperY1;
1160 taperY = 1.0f - taperY1; 1623 taperY = 1.0f - taperY1;
1161 } 1624 }
1162 else 1625 else
1163 { 1626 {
1164 taperX = _pbs.PathTaperX * 0.01f; 1627 taperX = _pbs.PathTaperX * 0.01f;
1165 if (taperX < 0.0f) 1628 if (taperX < 0.0f)
1166 taperX = -taperX; 1629 taperX = -taperX;
@@ -1168,10 +1631,10 @@ namespace OpenSim.Region.Physics.OdePlugin
1168 1631
1169 taperY = _pbs.PathTaperY * 0.01f; 1632 taperY = _pbs.PathTaperY * 0.01f;
1170 if (taperY < 0.0f) 1633 if (taperY < 0.0f)
1171 taperY = -taperY; 1634 taperY = -taperY;
1172 taperY1 = 1.0f - taperY; 1635 taperY1 = 1.0f - taperY;
1173 1636
1174 } 1637 }
1175 1638
1176 1639
1177 volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); 1640 volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY);
@@ -1180,7 +1643,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1180 pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; 1643 pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f;
1181 volume *= (pathEnd - pathBegin); 1644 volume *= (pathEnd - pathBegin);
1182 1645
1183// this is crude aproximation 1646 // this is crude aproximation
1184 profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; 1647 profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f;
1185 profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; 1648 profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f;
1186 volume *= (profileEnd - profileBegin); 1649 volume *= (profileEnd - profileBegin);
@@ -1189,8 +1652,8 @@ namespace OpenSim.Region.Physics.OdePlugin
1189 1652
1190 if (returnMass <= 0) 1653 if (returnMass <= 0)
1191 returnMass = 0.0001f;//ckrinke: Mass must be greater then zero. 1654 returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
1192// else if (returnMass > _parent_scene.maximumMassObject) 1655 // else if (returnMass > _parent_scene.maximumMassObject)
1193// returnMass = _parent_scene.maximumMassObject; 1656 // returnMass = _parent_scene.maximumMassObject;
1194 1657
1195 1658
1196 1659
@@ -1230,7 +1693,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1230 1693
1231 public void setMass() 1694 public void setMass()
1232 { 1695 {
1233 if (Body != (IntPtr) 0) 1696 if (Body != (IntPtr)0)
1234 { 1697 {
1235 float newmass = CalculateMass(); 1698 float newmass = CalculateMass();
1236 1699
@@ -1260,7 +1723,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1260 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 1723 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1261 } 1724 }
1262 1725
1263 1726
1264 d.BodyDestroy(Body); 1727 d.BodyDestroy(Body);
1265 lock (childrenPrim) 1728 lock (childrenPrim)
1266 { 1729 {
@@ -1279,7 +1742,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1279 else 1742 else
1280 { 1743 {
1281 _parent_scene.remActivePrim(this); 1744 _parent_scene.remActivePrim(this);
1282 1745
1283 m_collisionCategories &= ~CollisionCategories.Body; 1746 m_collisionCategories &= ~CollisionCategories.Body;
1284 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); 1747 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
1285 1748
@@ -1289,7 +1752,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1289 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 1752 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1290 } 1753 }
1291 1754
1292 1755
1293 Body = IntPtr.Zero; 1756 Body = IntPtr.Zero;
1294 } 1757 }
1295 } 1758 }
@@ -1345,10 +1808,10 @@ namespace OpenSim.Region.Physics.OdePlugin
1345 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1808 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1346 try 1809 try
1347 { 1810 {
1348 // if (prim_geom == IntPtr.Zero) // setGeom takes care of phys engine recreate and prim_geom pointer 1811 // if (prim_geom == IntPtr.Zero) // setGeom takes care of phys engine recreate and prim_geom pointer
1349 // { 1812 // {
1350 SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null)); 1813 SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
1351 // } 1814 // }
1352 } 1815 }
1353 catch (AccessViolationException) 1816 catch (AccessViolationException)
1354 { 1817 {
@@ -1357,14 +1820,14 @@ namespace OpenSim.Region.Physics.OdePlugin
1357 } 1820 }
1358 1821
1359 1822
1360 // if (IsPhysical && Body == (IntPtr) 0) 1823 // if (IsPhysical && Body == (IntPtr) 0)
1361 // { 1824 // {
1362 // Recreate the body 1825 // Recreate the body
1363 // m_interpenetrationcount = 0; 1826 // m_interpenetrationcount = 0;
1364 // m_collisionscore = 0; 1827 // m_collisionscore = 0;
1365 1828
1366 // enableBody(); 1829 // enableBody();
1367 // } 1830 // }
1368 } 1831 }
1369 1832
1370 public void ProcessTaints(float timestep) //============================================================================= 1833 public void ProcessTaints(float timestep) //=============================================================================
@@ -1373,37 +1836,37 @@ namespace OpenSim.Region.Physics.OdePlugin
1373 { 1836 {
1374 changeadd(timestep); 1837 changeadd(timestep);
1375 } 1838 }
1376 1839
1377 if (prim_geom != IntPtr.Zero) 1840 if (prim_geom != IntPtr.Zero)
1378 { 1841 {
1379 if (!_position.ApproxEquals(m_taintposition, 0f)) 1842 if (!_position.ApproxEquals(m_taintposition, 0f))
1380 { 1843 {
1381 changemove(timestep); 1844 changemove(timestep);
1382 } 1845 }
1383 if (m_taintrot != _orientation) 1846 if (m_taintrot != _orientation)
1384 { 1847 {
1385 if(childPrim && IsPhysical) // For physical child prim... 1848 if (childPrim && IsPhysical) // For physical child prim...
1386 { 1849 {
1387 rotate(timestep); 1850 rotate(timestep);
1388 // KF: ODE will also rotate the parent prim! 1851 // KF: ODE will also rotate the parent prim!
1389 // so rotate the root back to where it was 1852 // so rotate the root back to where it was
1390 OdePrim parent = (OdePrim)_parent; 1853 OdePrim parent = (OdePrim)_parent;
1391 parent.rotate(timestep); 1854 parent.rotate(timestep);
1392 } 1855 }
1393 else 1856 else
1394 { 1857 {
1395 //Just rotate the prim 1858 //Just rotate the prim
1396 rotate(timestep); 1859 rotate(timestep);
1397 } 1860 }
1398 } 1861 }
1399 // 1862 //
1400 1863
1401 if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent)) 1864 if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent))
1402 { 1865 {
1403 changePhysicsStatus(timestep); 1866 changePhysicsStatus(timestep);
1404 }// 1867 }//
1405 1868
1406 if (!_size.ApproxEquals(m_taintsize,0f)) 1869 if (!_size.ApproxEquals(m_taintsize, 0f))
1407 changesize(timestep); 1870 changesize(timestep);
1408 // 1871 //
1409 1872
@@ -1434,10 +1897,17 @@ namespace OpenSim.Region.Physics.OdePlugin
1434 1897
1435 if (m_taintCollidesWater != m_collidesWater) 1898 if (m_taintCollidesWater != m_collidesWater)
1436 changefloatonwater(timestep); 1899 changefloatonwater(timestep);
1437/* obsolete 1900
1438 if (!m_angularLock.ApproxEquals(m_taintAngularLock,0f)) 1901 if (m_taintvehicledata != null)
1439 changeAngularLock(timestep); 1902 DoSetVehicle();
1440 */ 1903
1904 if (m_taintserial != null)
1905 DoSerialize(m_taintserial);
1906
1907 /* obsolete
1908 if (!m_angularLock.ApproxEquals(m_taintAngularLock,0f))
1909 changeAngularLock(timestep);
1910 */
1441 } 1911 }
1442 else 1912 else
1443 { 1913 {
@@ -1445,16 +1915,16 @@ namespace OpenSim.Region.Physics.OdePlugin
1445 } 1915 }
1446 } 1916 }
1447 1917
1448/* obsolete 1918 /* obsolete
1449 private void changeAngularLock(float timestep) 1919 private void changeAngularLock(float timestep)
1450 { 1920 {
1451 if (_parent == null) 1921 if (_parent == null)
1452 { 1922 {
1453 m_angularLock = m_taintAngularLock; 1923 m_angularLock = m_taintAngularLock;
1454 m_angularLockSet = true; 1924 m_angularLockSet = true;
1455 } 1925 }
1456 } 1926 }
1457 */ 1927 */
1458 private void changelink(float timestep) 1928 private void changelink(float timestep)
1459 { 1929 {
1460 // If the newly set parent is not null 1930 // If the newly set parent is not null
@@ -1489,7 +1959,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1489 childPrim = false; 1959 childPrim = false;
1490 //_parent = null; 1960 //_parent = null;
1491 } 1961 }
1492 1962
1493 /* 1963 /*
1494 if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0) 1964 if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0)
1495 d.JointGroupDestroy(_linkJointGroup); 1965 d.JointGroupDestroy(_linkJointGroup);
@@ -1498,7 +1968,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1498 m_linkJoint = (IntPtr)0; 1968 m_linkJoint = (IntPtr)0;
1499 */ 1969 */
1500 } 1970 }
1501 1971
1502 _parent = m_taintparent; 1972 _parent = m_taintparent;
1503 m_taintPhysics = m_isphysical; 1973 m_taintPhysics = m_isphysical;
1504 } 1974 }
@@ -1512,8 +1982,8 @@ namespace OpenSim.Region.Physics.OdePlugin
1512 if (Body == IntPtr.Zero) 1982 if (Body == IntPtr.Zero)
1513 { 1983 {
1514 Body = d.BodyCreate(_parent_scene.world); 1984 Body = d.BodyCreate(_parent_scene.world);
1515 // disconnect from world gravity so we can apply buoyancy 1985 // disconnect from world gravity so we can apply buoyancy
1516 d.BodySetGravityMode (Body, false); 1986 d.BodySetGravityMode(Body, false);
1517 1987
1518 setMass(); 1988 setMass();
1519 } 1989 }
@@ -1524,7 +1994,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1524 if (!childrenPrim.Contains(prim)) 1994 if (!childrenPrim.Contains(prim))
1525 { 1995 {
1526 childrenPrim.Add(prim); 1996 childrenPrim.Add(prim);
1527 1997
1528 foreach (OdePrim prm in childrenPrim) 1998 foreach (OdePrim prm in childrenPrim)
1529 { 1999 {
1530 d.Mass m2; 2000 d.Mass m2;
@@ -1546,7 +2016,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1546 } 2016 }
1547 foreach (OdePrim prm in childrenPrim) 2017 foreach (OdePrim prm in childrenPrim)
1548 { 2018 {
1549 2019
1550 prm.m_collisionCategories |= CollisionCategories.Body; 2020 prm.m_collisionCategories |= CollisionCategories.Body;
1551 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); 2021 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1552 2022
@@ -1571,7 +2041,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1571 { 2041 {
1572 d.GeomSetBody(prm.prim_geom, Body); 2042 d.GeomSetBody(prm.prim_geom, Body);
1573 prm.childPrim = true; 2043 prm.childPrim = true;
1574 d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z); 2044 d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X, prm.Position.Y, prm.Position.Z);
1575 //d.GeomSetOffsetPosition(prim.prim_geom, 2045 //d.GeomSetOffsetPosition(prim.prim_geom,
1576 // (Position.X - prm.Position.X) - pMass.c.X, 2046 // (Position.X - prm.Position.X) - pMass.c.X,
1577 // (Position.Y - prm.Position.Y) - pMass.c.Y, 2047 // (Position.Y - prm.Position.Y) - pMass.c.Y,
@@ -1668,7 +2138,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1668 ParentPrim(prm); 2138 ParentPrim(prm);
1669 } 2139 }
1670 } 2140 }
1671 2141
1672 } 2142 }
1673 2143
1674 private void ChildDelink(OdePrim odePrim) 2144 private void ChildDelink(OdePrim odePrim)
@@ -1719,7 +2189,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1719 // in between the disabling and the collision properties setting 2189 // in between the disabling and the collision properties setting
1720 // which would wake the physical body up from a soft disabling and potentially cause it to fall 2190 // which would wake the physical body up from a soft disabling and potentially cause it to fall
1721 // through the ground. 2191 // through the ground.
1722 2192
1723 // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select 2193 // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select
1724 // just one part of the assembly, the rest of the assembly is non-selected and still simulating, 2194 // just one part of the assembly, the rest of the assembly is non-selected and still simulating,
1725 // so that causes the selected part to wake up and continue moving. 2195 // so that causes the selected part to wake up and continue moving.
@@ -1752,12 +2222,12 @@ namespace OpenSim.Region.Physics.OdePlugin
1752 { 2222 {
1753 disableBodySoft(); 2223 disableBodySoft();
1754 } 2224 }
1755 if (Body != IntPtr.Zero) 2225 if (Body != IntPtr.Zero)
1756 { 2226 {
1757 d.BodySetLinearVel(Body, 0f, 0f, 0f); 2227 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1758 d.BodySetForce(Body, 0f, 0f, 0f); 2228 d.BodySetForce(Body, 0f, 0f, 0f);
1759 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); 2229 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
1760 d.BodySetTorque (Body, 0.0f, 0.0f, 0.0f); 2230 d.BodySetTorque(Body, 0.0f, 0.0f, 0.0f);
1761 } 2231 }
1762 2232
1763 } 2233 }
@@ -1780,12 +2250,12 @@ namespace OpenSim.Region.Physics.OdePlugin
1780 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 2250 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1781 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 2251 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1782 } 2252 }
1783 if (Body != IntPtr.Zero) 2253 if (Body != IntPtr.Zero)
1784 { 2254 {
1785 d.BodySetLinearVel(Body, 0f, 0f, 0f); 2255 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1786 d.BodySetForce(Body, 0f, 0f, 0f); 2256 d.BodySetForce(Body, 0f, 0f, 0f);
1787 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); 2257 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
1788 d.BodySetTorque (Body, 0.0f, 0.0f, 0.0f); 2258 d.BodySetTorque(Body, 0.0f, 0.0f, 0.0f);
1789 } 2259 }
1790 2260
1791 if (m_isphysical) 2261 if (m_isphysical)
@@ -1913,7 +2383,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1913 m_meshfailed = true; 2383 m_meshfailed = true;
1914 } 2384 }
1915 // createmesh returns null when it's a shape that isn't a cube. 2385 // createmesh returns null when it's a shape that isn't a cube.
1916 // m_log.Debug(m_localID); 2386 // m_log.Debug(m_localID);
1917 } 2387 }
1918 } 2388 }
1919 2389
@@ -1948,7 +2418,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1948 { 2418 {
1949 if (m_isphysical) 2419 if (m_isphysical)
1950 { 2420 {
1951// if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits! 2421 // if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits!
1952 if (!m_taintremove && !childPrim) 2422 if (!m_taintremove && !childPrim)
1953 { 2423 {
1954 if (Body == IntPtr.Zero) 2424 if (Body == IntPtr.Zero)
@@ -1972,8 +2442,8 @@ namespace OpenSim.Region.Physics.OdePlugin
1972 OdePrim odParent = (OdePrim)_parent; 2442 OdePrim odParent = (OdePrim)_parent;
1973 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body) 2443 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
1974 { 2444 {
1975// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used?? 2445 // KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
1976Console.WriteLine("ODEPrim JointCreateFixed !!!"); 2446 Console.WriteLine("ODEPrim JointCreateFixed !!!");
1977 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); 2447 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
1978 d.JointAttach(m_linkJoint, Body, odParent.Body); 2448 d.JointAttach(m_linkJoint, Body, odParent.Body);
1979 d.JointSetFixed(m_linkJoint); 2449 d.JointSetFixed(m_linkJoint);
@@ -1991,8 +2461,8 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
1991 } 2461 }
1992 } 2462 }
1993 //else 2463 //else
1994 // { 2464 // {
1995 //m_log.Debug("[BUG]: race!"); 2465 //m_log.Debug("[BUG]: race!");
1996 //} 2466 //}
1997 } 2467 }
1998 else 2468 else
@@ -2031,15 +2501,15 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2031 myrot.W = _orientation.W; 2501 myrot.W = _orientation.W;
2032 if (Body != IntPtr.Zero) 2502 if (Body != IntPtr.Zero)
2033 { 2503 {
2034 // KF: If this is a root prim do BodySet 2504 // KF: If this is a root prim do BodySet
2035 d.BodySetQuaternion(Body, ref myrot); 2505 d.BodySetQuaternion(Body, ref myrot);
2036 }
2037 else
2038 {
2039 // daughter prim, do Geom set
2040 d.GeomSetQuaternion(prim_geom, ref myrot);
2041 } 2506 }
2042 2507 else
2508 {
2509 // daughter prim, do Geom set
2510 d.GeomSetQuaternion(prim_geom, ref myrot);
2511 }
2512
2043 resetCollisionAccounting(); 2513 resetCollisionAccounting();
2044 m_taintrot = _orientation; 2514 m_taintrot = _orientation;
2045 } 2515 }
@@ -2111,7 +2581,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2111 2581
2112 public void changesize(float timestamp) 2582 public void changesize(float timestamp)
2113 { 2583 {
2114 2584
2115 string oldname = _parent_scene.geom_name_map[prim_geom]; 2585 string oldname = _parent_scene.geom_name_map[prim_geom];
2116 2586
2117 if (_size.X <= 0) _size.X = 0.01f; 2587 if (_size.X <= 0) _size.X = 0.01f;
@@ -2170,7 +2640,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2170 //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); 2640 //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
2171 CreateGeom(m_targetSpace, mesh); 2641 CreateGeom(m_targetSpace, mesh);
2172 2642
2173 2643
2174 } 2644 }
2175 else 2645 else
2176 { 2646 {
@@ -2210,7 +2680,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2210 m_taintsize = _size; 2680 m_taintsize = _size;
2211 } 2681 }
2212 2682
2213 2683
2214 2684
2215 public void changefloatonwater(float timestep) 2685 public void changefloatonwater(float timestep)
2216 { 2686 {
@@ -2398,7 +2868,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2398 } 2868 }
2399 d.BodyEnable(Body); 2869 d.BodyEnable(Body);
2400 d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z); 2870 d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z);
2401 2871
2402 } 2872 }
2403 m_angularforcelist.Clear(); 2873 m_angularforcelist.Clear();
2404 } 2874 }
@@ -2420,7 +2890,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2420 if (Body != IntPtr.Zero) 2890 if (Body != IntPtr.Zero)
2421 d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z); 2891 d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
2422 } 2892 }
2423 2893
2424 //resetCollisionAccounting(); 2894 //resetCollisionAccounting();
2425 } 2895 }
2426 m_taintVelocity = Vector3.Zero; 2896 m_taintVelocity = Vector3.Zero;
@@ -2428,9 +2898,9 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2428 2898
2429 public void UpdatePositionAndVelocity() 2899 public void UpdatePositionAndVelocity()
2430 { 2900 {
2431 return; // moved to the Move () method 2901 return; // moved to the Move () method
2432 } 2902 }
2433 2903
2434 public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj) 2904 public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
2435 { 2905 {
2436 obj.I.M00 = pMat[0, 0]; 2906 obj.I.M00 = pMat[0, 0];
@@ -2455,7 +2925,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2455 { 2925 {
2456 _parent_scene.remCollisionEventReporting(this); 2926 _parent_scene.remCollisionEventReporting(this);
2457 m_eventsubscription = 0; 2927 m_eventsubscription = 0;
2458 } 2928 }
2459 2929
2460 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) 2930 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
2461 { 2931 {
@@ -2499,9 +2969,9 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2499 public static Matrix4 Adjoint(Matrix4 pMat) 2969 public static Matrix4 Adjoint(Matrix4 pMat)
2500 { 2970 {
2501 Matrix4 adjointMatrix = new Matrix4(); 2971 Matrix4 adjointMatrix = new Matrix4();
2502 for (int i=0; i<4; i++) 2972 for (int i = 0; i < 4; i++)
2503 { 2973 {
2504 for (int j=0; j<4; j++) 2974 for (int j = 0; j < 4; j++)
2505 { 2975 {
2506 Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j))))); 2976 Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j)))));
2507 } 2977 }
@@ -2524,7 +2994,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2524 { 2994 {
2525 if (j == iCol) 2995 if (j == iCol)
2526 continue; 2996 continue;
2527 Matrix4SetValue(ref minor, m,n, matrix[i, j]); 2997 Matrix4SetValue(ref minor, m, n, matrix[i, j]);
2528 n++; 2998 n++;
2529 } 2999 }
2530 m++; 3000 m++;
@@ -2622,18 +3092,18 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2622 private static float determinant3x3(Matrix4 pMat) 3092 private static float determinant3x3(Matrix4 pMat)
2623 { 3093 {
2624 float det = 0; 3094 float det = 0;
2625 float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2]; 3095 float diag1 = pMat[0, 0] * pMat[1, 1] * pMat[2, 2];
2626 float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0]; 3096 float diag2 = pMat[0, 1] * pMat[2, 1] * pMat[2, 0];
2627 float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1]; 3097 float diag3 = pMat[0, 2] * pMat[1, 0] * pMat[2, 1];
2628 float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2]; 3098 float diag4 = pMat[2, 0] * pMat[1, 1] * pMat[0, 2];
2629 float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0]; 3099 float diag5 = pMat[2, 1] * pMat[1, 2] * pMat[0, 0];
2630 float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1]; 3100 float diag6 = pMat[2, 2] * pMat[1, 0] * pMat[0, 1];
2631 3101
2632 det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6); 3102 det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6);
2633 return det; 3103 return det;
2634 3104
2635 } 3105 }
2636 3106
2637 private static void DMassCopy(ref d.Mass src, ref d.Mass dst) 3107 private static void DMassCopy(ref d.Mass src, ref d.Mass dst)
2638 { 3108 {
2639 dst.c.W = src.c.W; 3109 dst.c.W = src.c.W;
@@ -2690,15 +3160,15 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2690 // m_bankingTimescale = pValue; 3160 // m_bankingTimescale = pValue;
2691 break; 3161 break;
2692 case Vehicle.BUOYANCY: 3162 case Vehicle.BUOYANCY:
2693 if (pValue < -1f) pValue = -1f; 3163 if (pValue < -1f) pValue = -1f;
2694 if (pValue > 1f) pValue = 1f; 3164 if (pValue > 1f) pValue = 1f;
2695 m_VehicleBuoyancy = pValue; 3165 m_VehicleBuoyancy = pValue;
2696 break; 3166 break;
2697// case Vehicle.HOVER_EFFICIENCY: 3167 // case Vehicle.HOVER_EFFICIENCY:
2698// if (pValue < 0f) pValue = 0f; 3168 // if (pValue < 0f) pValue = 0f;
2699// if (pValue > 1f) pValue = 1f; 3169 // if (pValue > 1f) pValue = 1f;
2700// m_VhoverEfficiency = pValue; 3170 // m_VhoverEfficiency = pValue;
2701// break; 3171 // break;
2702 case Vehicle.HOVER_HEIGHT: 3172 case Vehicle.HOVER_HEIGHT:
2703 m_VhoverHeight = pValue; 3173 m_VhoverHeight = pValue;
2704 break; 3174 break;
@@ -2731,12 +3201,12 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2731 if (pValue < 0.1f) pValue = 0.1f; 3201 if (pValue < 0.1f) pValue = 0.1f;
2732 m_verticalAttractionTimescale = pValue; 3202 m_verticalAttractionTimescale = pValue;
2733 break; 3203 break;
2734 3204
2735 // These are vector properties but the engine lets you use a single float value to 3205 // These are vector properties but the engine lets you use a single float value to
2736 // set all of the components to the same value 3206 // set all of the components to the same value
2737 case Vehicle.ANGULAR_FRICTION_TIMESCALE: 3207 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
2738 if (pValue > 30f) pValue = 30f; 3208 if (pValue > 30f) pValue = 30f;
2739 if (pValue < 0.1f) pValue = 0.1f; 3209 if (pValue < 0.1f) pValue = 0.1f;
2740 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); 3210 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
2741 break; 3211 break;
2742 case Vehicle.ANGULAR_MOTOR_DIRECTION: 3212 case Vehicle.ANGULAR_MOTOR_DIRECTION:
@@ -2744,7 +3214,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2744 UpdateAngDecay(); 3214 UpdateAngDecay();
2745 break; 3215 break;
2746 case Vehicle.LINEAR_FRICTION_TIMESCALE: 3216 case Vehicle.LINEAR_FRICTION_TIMESCALE:
2747 if (pValue < 0.1f) pValue = 0.1f; 3217 if (pValue < 0.1f) pValue = 0.1f;
2748 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); 3218 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
2749 break; 3219 break;
2750 case Vehicle.LINEAR_MOTOR_DIRECTION: 3220 case Vehicle.LINEAR_MOTOR_DIRECTION:
@@ -2756,7 +3226,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2756 break; 3226 break;
2757 3227
2758 } 3228 }
2759 3229
2760 }//end ProcessFloatVehicleParam 3230 }//end ProcessFloatVehicleParam
2761 3231
2762 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) 3232 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
@@ -2764,29 +3234,29 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2764 switch (pParam) 3234 switch (pParam)
2765 { 3235 {
2766 case Vehicle.ANGULAR_FRICTION_TIMESCALE: 3236 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
2767 if (pValue.X > 30f) pValue.X = 30f; 3237 if (pValue.X > 30f) pValue.X = 30f;
2768 if (pValue.X < 0.1f) pValue.X = 0.1f; 3238 if (pValue.X < 0.1f) pValue.X = 0.1f;
2769 if (pValue.Y > 30f) pValue.Y = 30f; 3239 if (pValue.Y > 30f) pValue.Y = 30f;
2770 if (pValue.Y < 0.1f) pValue.Y = 0.1f; 3240 if (pValue.Y < 0.1f) pValue.Y = 0.1f;
2771 if (pValue.Z > 30f) pValue.Z = 30f; 3241 if (pValue.Z > 30f) pValue.Z = 30f;
2772 if (pValue.Z < 0.1f) pValue.Z = 0.1f; 3242 if (pValue.Z < 0.1f) pValue.Z = 0.1f;
2773 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); 3243 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
2774 break; 3244 break;
2775 case Vehicle.ANGULAR_MOTOR_DIRECTION: 3245 case Vehicle.ANGULAR_MOTOR_DIRECTION:
2776 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); 3246 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
2777 // Limit requested angular speed to 2 rps= 4 pi rads/sec 3247 // Limit requested angular speed to 2 rps= 4 pi rads/sec
2778 if(m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f; 3248 if (m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f;
2779 if(m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f; 3249 if (m_angularMotorDirection.X < -12.56f) m_angularMotorDirection.X = -12.56f;
2780 if(m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f; 3250 if (m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f;
2781 if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f; 3251 if (m_angularMotorDirection.Y < -12.56f) m_angularMotorDirection.Y = -12.56f;
2782 if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f; 3252 if (m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f;
2783 if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f; 3253 if (m_angularMotorDirection.Z < -12.56f) m_angularMotorDirection.Z = -12.56f;
2784 UpdateAngDecay(); 3254 UpdateAngDecay();
2785 break; 3255 break;
2786 case Vehicle.LINEAR_FRICTION_TIMESCALE: 3256 case Vehicle.LINEAR_FRICTION_TIMESCALE:
2787 if (pValue.X < 0.1f) pValue.X = 0.1f; 3257 if (pValue.X < 0.1f) pValue.X = 0.1f;
2788 if (pValue.Y < 0.1f) pValue.Y = 0.1f; 3258 if (pValue.Y < 0.1f) pValue.Y = 0.1f;
2789 if (pValue.Z < 0.1f) pValue.Z = 0.1f; 3259 if (pValue.Z < 0.1f) pValue.Z = 0.1f;
2790 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); 3260 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
2791 break; 3261 break;
2792 case Vehicle.LINEAR_MOTOR_DIRECTION: 3262 case Vehicle.LINEAR_MOTOR_DIRECTION:
@@ -2797,7 +3267,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2797 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); 3267 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
2798 break; 3268 break;
2799 } 3269 }
2800 3270
2801 }//end ProcessVectorVehicleParam 3271 }//end ProcessVectorVehicleParam
2802 3272
2803 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) 3273 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
@@ -2808,31 +3278,31 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2808 // m_referenceFrame = pValue; 3278 // m_referenceFrame = pValue;
2809 break; 3279 break;
2810 } 3280 }
2811 3281
2812 }//end ProcessRotationVehicleParam 3282 }//end ProcessRotationVehicleParam
2813 3283
2814 internal void ProcessVehicleFlags(int pParam, bool remove) 3284 internal void ProcessVehicleFlags(int pParam, bool remove)
2815 { 3285 {
2816 if (remove) 3286 if (remove)
2817 { 3287 {
2818 m_flags &= ~((VehicleFlag)pParam); 3288 m_flags &= ~((VehicleFlag)pParam);
2819 } 3289 }
2820 else 3290 else
2821 { 3291 {
2822 m_flags |= (VehicleFlag)pParam; 3292 m_flags |= (VehicleFlag)pParam;
2823 } 3293 }
2824 } 3294 }
2825 3295
2826 internal void ProcessTypeChange(Vehicle pType) 3296 internal void ProcessTypeChange(Vehicle pType)
2827 { 3297 {
2828 // Set Defaults For Type 3298 // Set Defaults For Type
2829 m_type = pType; 3299 m_type = pType;
2830 switch (pType) 3300 switch (pType)
2831 { 3301 {
2832 case Vehicle.TYPE_SLED: 3302 case Vehicle.TYPE_SLED:
2833 m_linearFrictionTimescale = new Vector3(30, 1, 1000); 3303 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
2834 m_angularFrictionTimescale = new Vector3(30, 30, 30); 3304 m_angularFrictionTimescale = new Vector3(30, 30, 30);
2835// m_lLinMotorVel = Vector3.Zero; 3305 // m_lLinMotorVel = Vector3.Zero;
2836 m_linearMotorTimescale = 1000; 3306 m_linearMotorTimescale = 1000;
2837 m_linearMotorDecayTimescale = 120; 3307 m_linearMotorDecayTimescale = 120;
2838 m_angularMotorDirection = Vector3.Zero; 3308 m_angularMotorDirection = Vector3.Zero;
@@ -2840,7 +3310,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2840 m_angularMotorTimescale = 1000; 3310 m_angularMotorTimescale = 1000;
2841 m_angularMotorDecayTimescale = 120; 3311 m_angularMotorDecayTimescale = 120;
2842 m_VhoverHeight = 0; 3312 m_VhoverHeight = 0;
2843// m_VhoverEfficiency = 1; 3313 // m_VhoverEfficiency = 1;
2844 m_VhoverTimescale = 10; 3314 m_VhoverTimescale = 10;
2845 m_VehicleBuoyancy = 0; 3315 m_VehicleBuoyancy = 0;
2846 // m_linearDeflectionEfficiency = 1; 3316 // m_linearDeflectionEfficiency = 1;
@@ -2859,7 +3329,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2859 case Vehicle.TYPE_CAR: 3329 case Vehicle.TYPE_CAR:
2860 m_linearFrictionTimescale = new Vector3(100, 2, 1000); 3330 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
2861 m_angularFrictionTimescale = new Vector3(30, 30, 30); // was 1000, but sl max frict time is 30. 3331 m_angularFrictionTimescale = new Vector3(30, 30, 30); // was 1000, but sl max frict time is 30.
2862// m_lLinMotorVel = Vector3.Zero; 3332 // m_lLinMotorVel = Vector3.Zero;
2863 m_linearMotorTimescale = 1; 3333 m_linearMotorTimescale = 1;
2864 m_linearMotorDecayTimescale = 60; 3334 m_linearMotorDecayTimescale = 60;
2865 m_angularMotorDirection = Vector3.Zero; 3335 m_angularMotorDirection = Vector3.Zero;
@@ -2867,7 +3337,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2867 m_angularMotorTimescale = 1; 3337 m_angularMotorTimescale = 1;
2868 m_angularMotorDecayTimescale = 0.8f; 3338 m_angularMotorDecayTimescale = 0.8f;
2869 m_VhoverHeight = 0; 3339 m_VhoverHeight = 0;
2870// m_VhoverEfficiency = 0; 3340 // m_VhoverEfficiency = 0;
2871 m_VhoverTimescale = 1000; 3341 m_VhoverTimescale = 1000;
2872 m_VehicleBuoyancy = 0; 3342 m_VehicleBuoyancy = 0;
2873 // // m_linearDeflectionEfficiency = 1; 3343 // // m_linearDeflectionEfficiency = 1;
@@ -2886,8 +3356,8 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2886 break; 3356 break;
2887 case Vehicle.TYPE_BOAT: 3357 case Vehicle.TYPE_BOAT:
2888 m_linearFrictionTimescale = new Vector3(10, 3, 2); 3358 m_linearFrictionTimescale = new Vector3(10, 3, 2);
2889 m_angularFrictionTimescale = new Vector3(10,10,10); 3359 m_angularFrictionTimescale = new Vector3(10, 10, 10);
2890// m_lLinMotorVel = Vector3.Zero; 3360 // m_lLinMotorVel = Vector3.Zero;
2891 m_linearMotorTimescale = 5; 3361 m_linearMotorTimescale = 5;
2892 m_linearMotorDecayTimescale = 60; 3362 m_linearMotorDecayTimescale = 60;
2893 m_angularMotorDirection = Vector3.Zero; 3363 m_angularMotorDirection = Vector3.Zero;
@@ -2895,7 +3365,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2895 m_angularMotorTimescale = 4; 3365 m_angularMotorTimescale = 4;
2896 m_angularMotorDecayTimescale = 4; 3366 m_angularMotorDecayTimescale = 4;
2897 m_VhoverHeight = 0; 3367 m_VhoverHeight = 0;
2898// m_VhoverEfficiency = 0.5f; 3368 // m_VhoverEfficiency = 0.5f;
2899 m_VhoverTimescale = 2; 3369 m_VhoverTimescale = 2;
2900 m_VehicleBuoyancy = 1; 3370 m_VehicleBuoyancy = 1;
2901 // m_linearDeflectionEfficiency = 0.5f; 3371 // m_linearDeflectionEfficiency = 0.5f;
@@ -2908,15 +3378,15 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2908 // m_bankingMix = 0.8f; 3378 // m_bankingMix = 0.8f;
2909 // m_bankingTimescale = 1; 3379 // m_bankingTimescale = 1;
2910 // m_referenceFrame = Quaternion.Identity; 3380 // m_referenceFrame = Quaternion.Identity;
2911 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY | 3381 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY |
2912 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); 3382 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
2913 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | 3383 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY |
2914 VehicleFlag.LIMIT_MOTOR_UP); 3384 VehicleFlag.LIMIT_MOTOR_UP);
2915 break; 3385 break;
2916 case Vehicle.TYPE_AIRPLANE: 3386 case Vehicle.TYPE_AIRPLANE:
2917 m_linearFrictionTimescale = new Vector3(200, 10, 5); 3387 m_linearFrictionTimescale = new Vector3(200, 10, 5);
2918 m_angularFrictionTimescale = new Vector3(20, 20, 20); 3388 m_angularFrictionTimescale = new Vector3(20, 20, 20);
2919// m_lLinMotorVel = Vector3.Zero; 3389 // m_lLinMotorVel = Vector3.Zero;
2920 m_linearMotorTimescale = 2; 3390 m_linearMotorTimescale = 2;
2921 m_linearMotorDecayTimescale = 60; 3391 m_linearMotorDecayTimescale = 60;
2922 m_angularMotorDirection = Vector3.Zero; 3392 m_angularMotorDirection = Vector3.Zero;
@@ -2924,7 +3394,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2924 m_angularMotorTimescale = 4; 3394 m_angularMotorTimescale = 4;
2925 m_angularMotorDecayTimescale = 4; 3395 m_angularMotorDecayTimescale = 4;
2926 m_VhoverHeight = 0; 3396 m_VhoverHeight = 0;
2927// m_VhoverEfficiency = 0.5f; 3397 // m_VhoverEfficiency = 0.5f;
2928 m_VhoverTimescale = 1000; 3398 m_VhoverTimescale = 1000;
2929 m_VehicleBuoyancy = 0; 3399 m_VehicleBuoyancy = 0;
2930 // m_linearDeflectionEfficiency = 0.5f; 3400 // m_linearDeflectionEfficiency = 0.5f;
@@ -2951,7 +3421,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2951 m_angularMotorTimescale = 6; 3421 m_angularMotorTimescale = 6;
2952 m_angularMotorDecayTimescale = 10; 3422 m_angularMotorDecayTimescale = 10;
2953 m_VhoverHeight = 5; 3423 m_VhoverHeight = 5;
2954// m_VhoverEfficiency = 0.8f; 3424 // m_VhoverEfficiency = 0.8f;
2955 m_VhoverTimescale = 10; 3425 m_VhoverTimescale = 10;
2956 m_VehicleBuoyancy = 1; 3426 m_VehicleBuoyancy = 1;
2957 // m_linearDeflectionEfficiency = 0; 3427 // m_linearDeflectionEfficiency = 0;
@@ -2981,63 +3451,48 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2981 } 3451 }
2982 3452
2983 3453
2984 internal void Halt() 3454 internal void Halt()
2985 { // Kill all motions, when non-physical 3455 { // Kill all motions, when non-physical
2986 // m_linearMotorDirection = Vector3.Zero; 3456 // m_linearMotorDirection = Vector3.Zero;
2987 m_lLinMotorDVel = Vector3.Zero; 3457 m_lLinMotorDVel = Vector3.Zero;
2988 m_lLinObjectVel = Vector3.Zero; 3458 m_lLinObjectVel = Vector3.Zero;
2989 m_wLinObjectVel = Vector3.Zero; 3459 m_wLinObjectVel = Vector3.Zero;
2990 m_angularMotorDirection = Vector3.Zero; 3460 m_angularMotorDirection = Vector3.Zero;
2991 m_lastAngularVelocity = Vector3.Zero; 3461 m_lastAngularVelocity = Vector3.Zero;
2992 m_angularMotorDVel = Vector3.Zero; 3462 m_angularMotorDVel = Vector3.Zero;
2993 _acceleration = Vector3.Zero; 3463 _acceleration = Vector3.Zero;
2994 } 3464 }
2995 3465
2996 private void UpdateLinDecay() 3466 private void UpdateLinDecay()
2997 { 3467 {
2998// if (Math.Abs(m_linearMotorDirection.X) > Math.Abs(m_lLinMotorDVel.X)) m_lLinMotorDVel.X = m_linearMotorDirection.X; 3468 m_lLinMotorDVel.X = m_linearMotorDirection.X;
2999// if (Math.Abs(m_linearMotorDirection.Y) > Math.Abs(m_lLinMotorDVel.Y)) m_lLinMotorDVel.Y = m_linearMotorDirection.Y; 3469 m_lLinMotorDVel.Y = m_linearMotorDirection.Y;
3000// if (Math.Abs(m_linearMotorDirection.Z) > Math.Abs(m_lLinMotorDVel.Z)) m_lLinMotorDVel.Z = m_linearMotorDirection.Z; 3470 m_lLinMotorDVel.Z = m_linearMotorDirection.Z;
3001 m_lLinMotorDVel.X = m_linearMotorDirection.X; 3471 } // else let the motor decay on its own
3002 m_lLinMotorDVel.Y = m_linearMotorDirection.Y; 3472
3003 m_lLinMotorDVel.Z = m_linearMotorDirection.Z; 3473 private void UpdateAngDecay()
3004 } // else let the motor decay on its own 3474 {
3005 3475 m_angularMotorDVel.X = m_angularMotorDirection.X;
3006 private void UpdateAngDecay() 3476 m_angularMotorDVel.Y = m_angularMotorDirection.Y;
3007 { 3477 m_angularMotorDVel.Z = m_angularMotorDirection.Z;
3008// if (Math.Abs(m_angularMotorDirection.X) > Math.Abs(m_angularMotorDVel.X)) m_angularMotorDVel.X = m_angularMotorDirection.X; 3478 } // else let the motor decay on its own
3009// if (Math.Abs(m_angularMotorDirection.Y) > Math.Abs(m_angularMotorDVel.Y)) m_angularMotorDVel.Y = m_angularMotorDirection.Y; 3479
3010// if (Math.Abs(m_angularMotorDirection.Z) > Math.Abs(m_angularMotorDVel.Z)) m_angularMotorDVel.Z = m_angularMotorDirection.Z;
3011 m_angularMotorDVel.X = m_angularMotorDirection.X;
3012 m_angularMotorDVel.Y = m_angularMotorDirection.Y;
3013 m_angularMotorDVel.Z = m_angularMotorDirection.Z;
3014 } // else let the motor decay on its own
3015
3016 public void Move(float timestep) 3480 public void Move(float timestep)
3017 { 3481 {
3018 float fx = 0; 3482 float fx = 0;
3019 float fy = 0; 3483 float fy = 0;
3020 float fz = 0; 3484 float fz = 0;
3021 Vector3 linvel; // velocity applied, including any reversal 3485 Vector3 linvel; // velocity applied, including any reversal
3022 int outside = 0; 3486
3023
3024 // If geomCrossingFailuresBeforeOutofbounds is set to 0 in OpenSim.ini then phys objects bounce off region borders. 3487 // If geomCrossingFailuresBeforeOutofbounds is set to 0 in OpenSim.ini then phys objects bounce off region borders.
3025 // This is a temp patch until proper region crossing is developed. 3488 // This is a temp patch until proper region crossing is developed.
3026 3489
3027 int failureLimit = _parent_scene.geomCrossingFailuresBeforeOutofbounds; 3490
3028 float fence = _parent_scene.geomRegionFence;
3029
3030 frcount++; // used to limit debug comment output
3031 if (frcount > 50)
3032 frcount = 0;
3033
3034 if(revcount > 0) revcount--;
3035
3036 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim && !m_outofBounds) // Only move root prims. 3491 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim && !m_outofBounds) // Only move root prims.
3037 { 3492 {
3038 // Old public void UpdatePositionAndVelocity(), more accuratley calculated here 3493 // Old public void UpdatePositionAndVelocity(), more accuratley calculated here
3039 bool lastZeroFlag = _zeroFlag; // was it stopped 3494 bool lastZeroFlag = _zeroFlag; // was it stopped
3040 3495
3041 d.Vector3 vec = d.BodyGetPosition(Body); 3496 d.Vector3 vec = d.BodyGetPosition(Body);
3042 Vector3 l_position = Vector3.Zero; 3497 Vector3 l_position = Vector3.Zero;
3043 l_position.X = vec.X; 3498 l_position.X = vec.X;
@@ -3045,95 +3500,26 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
3045 l_position.Z = vec.Z; 3500 l_position.Z = vec.Z;
3046 m_lastposition = _position; 3501 m_lastposition = _position;
3047 _position = l_position; 3502 _position = l_position;
3048 3503
3049 d.Quaternion ori = d.BodyGetQuaternion(Body); 3504 d.Quaternion ori = d.BodyGetQuaternion(Body);
3050 // Quaternion l_orientation = Quaternion.Identity; 3505 // Quaternion l_orientation = Quaternion.Identity;
3051 _orientation.X = ori.X; 3506 _orientation.X = ori.X;
3052 _orientation.Y = ori.Y; 3507 _orientation.Y = ori.Y;
3053 _orientation.Z = ori.Z; 3508 _orientation.Z = ori.Z;
3054 _orientation.W = ori.W; 3509 _orientation.W = ori.W;
3055 m_lastorientation = _orientation; 3510 m_lastorientation = _orientation;
3056 3511
3057 d.Vector3 vel = d.BodyGetLinearVel(Body); 3512 d.Vector3 vel = d.BodyGetLinearVel(Body);
3058 m_lastVelocity = _velocity; 3513 m_lastVelocity = _velocity;
3059 _velocity.X = vel.X; 3514 _velocity.X = vel.X;
3060 _velocity.Y = vel.Y; 3515 _velocity.Y = vel.Y;
3061 _velocity.Z = vel.Z; 3516 _velocity.Z = vel.Z;
3062 _acceleration = ((_velocity - m_lastVelocity) / timestep); 3517 _acceleration = ((_velocity - m_lastVelocity) / timestep);
3063 3518
3064 d.Vector3 torque = d.BodyGetTorque(Body); 3519 d.Vector3 torque = d.BodyGetTorque(Body);
3065 _torque = new Vector3(torque.X, torque.Y, torque.Z); 3520 _torque = new Vector3(torque.X, torque.Y, torque.Z);
3066 3521
3067 3522
3068//Console.WriteLine("Move {0} at {1}", m_primName, l_position);
3069 /*
3070 // Check if outside region
3071 // In Scene.cs/CrossPrimGroupIntoNewRegion the object is checked for 0.1M from border!
3072 if (l_position.X > ((float)_parent_scene.WorldExtents.X - fence))
3073 {
3074 l_position.X = ((float)_parent_scene.WorldExtents.X - fence);
3075 outside = 1;
3076 }
3077
3078 if (l_position.X < fence)
3079 {
3080 l_position.X = fence;
3081 outside = 2;
3082 }
3083 if (l_position.Y > ((float)_parent_scene.WorldExtents.Y - fence))
3084 {
3085 l_position.Y = ((float)_parent_scene.WorldExtents.Y - fence);
3086 outside = 3;
3087 }
3088
3089 if (l_position.Y < fence)
3090 {
3091 l_position.Y = fence;
3092 outside = 4;
3093 }
3094
3095 if (outside > 0)
3096 {
3097
3098//Console.WriteLine("Border {0} fence={1}", l_position, fence);
3099 if (fence > 0.0f) // bounce object off boundary
3100 {
3101 if (revcount == 0)
3102 {
3103 if (outside < 3)
3104 {
3105 _velocity.X = -_velocity.X;
3106 }
3107 else
3108 {
3109 _velocity.Y = -_velocity.Y;
3110 }
3111 if (m_type != Vehicle.TYPE_NONE) Halt();
3112 _position = l_position;
3113 m_taintposition = _position;
3114 m_lastVelocity = _velocity;
3115 _acceleration = Vector3.Zero;
3116 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
3117 d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
3118 base.RequestPhysicsterseUpdate();
3119
3120 revcount = 25; // wait for object to move away from border
3121 }
3122 } // else old crossing mode
3123 else if (m_crossingfailures < failureLimit)
3124 { // keep trying to cross?
3125 _position = l_position;
3126 //_parent_scene.remActivePrim(this);
3127 if (_parent == null) base.RequestPhysicsterseUpdate();
3128 return; // Dont process any other motion?
3129 }
3130 else
3131 { // Too many tries
3132 if (_parent == null) base.RaiseOutOfBounds(l_position);
3133 return; // Dont process any other motion?
3134 } // end various methods
3135 } // end outside region horizontally
3136 */
3137 if (_position.X < 0f || _position.X > _parent_scene.WorldExtents.X 3523 if (_position.X < 0f || _position.X > _parent_scene.WorldExtents.X
3138 || _position.Y < 0f || _position.Y > _parent_scene.WorldExtents.Y 3524 || _position.Y < 0f || _position.Y > _parent_scene.WorldExtents.Y
3139 ) 3525 )
@@ -3146,41 +3532,11 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
3146 _position.Z = Util.Clip(l_position.Z, -100f, 50000f); 3532 _position.Z = Util.Clip(l_position.Z, -100f, 50000f);
3147 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); 3533 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
3148 d.BodySetLinearVel(Body, 0, 0, 0); 3534 d.BodySetLinearVel(Body, 0, 0, 0);
3149 /*
3150 if (Interlocked.Exchange(ref m_crossingfailures, m_crossingfailures) == 0)
3151 { // tell base code only once
3152 Interlocked.Increment(ref m_crossingfailures);
3153 base.RequestPhysicsterseUpdate();
3154 }
3155 */
3156 m_outofBounds = true; 3535 m_outofBounds = true;
3157 base.RequestPhysicsterseUpdate(); 3536 base.RequestPhysicsterseUpdate();
3158 return; 3537 return;
3159 } 3538 }
3160/*
3161 if (Interlocked.Exchange(ref m_crossingfailures, 0) != 0)
3162 {
3163 // main simulator had a crossing failure
3164 // park it inside region
3165 _position.X = Util.Clip(l_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);
3166 _position.Y = Util.Clip(l_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);
3167 _position.Z = Util.Clip(l_position.Z, -100f, 50000f);
3168 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
3169 3539
3170 m_lastposition = _position;
3171
3172 _velocity = Vector3.Zero;
3173 m_lastVelocity = _velocity;
3174
3175
3176 if (m_type != Vehicle.TYPE_NONE)
3177 Halt();
3178
3179 d.BodySetLinearVel(Body, 0, 0, 0);
3180 base.RequestPhysicsterseUpdate();
3181 return;
3182 }
3183*/
3184 base.RequestPhysicsterseUpdate(); 3540 base.RequestPhysicsterseUpdate();
3185 3541
3186 if (l_position.Z < 0) 3542 if (l_position.Z < 0)
@@ -3193,8 +3549,8 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
3193 3549
3194 //IsPhysical = false; 3550 //IsPhysical = false;
3195 if (_parent == null) base.RaiseOutOfBounds(_position); 3551 if (_parent == null) base.RaiseOutOfBounds(_position);
3196 3552
3197 3553
3198 _acceleration.X = 0; // This stuff may stop client display but it has no 3554 _acceleration.X = 0; // This stuff may stop client display but it has no
3199 _acceleration.Y = 0; // effect on the object in phys engine! 3555 _acceleration.Y = 0; // effect on the object in phys engine!
3200 _acceleration.Z = 0; 3556 _acceleration.Z = 0;
@@ -3215,13 +3571,13 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
3215 //outofBounds = true; 3571 //outofBounds = true;
3216 } // end neg Z check 3572 } // end neg Z check
3217 3573
3218 // Is it moving? 3574 // Is it moving?
3219 /* if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) 3575 /* if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
3220 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) 3576 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
3221 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) */ 3577 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) */
3222 if ( (Vector3.Mag(_velocity) < 0.01) && // moving very slowly 3578 if ((Vector3.Mag(_velocity) < 0.01) && // moving very slowly
3223 (Vector3.Mag(_velocity) < Vector3.Mag(m_lastVelocity)) && // decelerating 3579 (Vector3.Mag(_velocity) < Vector3.Mag(m_lastVelocity)) && // decelerating
3224 (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001) ) // spinning very slowly 3580 (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001)) // spinning very slowly
3225 { 3581 {
3226 _zeroFlag = true; 3582 _zeroFlag = true;
3227 m_throttleUpdates = false; 3583 m_throttleUpdates = false;
@@ -3238,18 +3594,18 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
3238 { // Its stopped 3594 { // Its stopped
3239 _velocity.X = 0.0f; 3595 _velocity.X = 0.0f;
3240 _velocity.Y = 0.0f; 3596 _velocity.Y = 0.0f;
3241 // _velocity.Z = 0.0f; 3597 // _velocity.Z = 0.0f;
3242 3598
3243 _acceleration.X = 0; 3599 _acceleration.X = 0;
3244 _acceleration.Y = 0; 3600 _acceleration.Y = 0;
3245 // _acceleration.Z = 0; 3601 // _acceleration.Z = 0;
3246 3602
3247 m_rotationalVelocity.X = 0; 3603 m_rotationalVelocity.X = 0;
3248 m_rotationalVelocity.Y = 0; 3604 m_rotationalVelocity.Y = 0;
3249 m_rotationalVelocity.Z = 0; 3605 m_rotationalVelocity.Z = 0;
3250 // Stop it in the phys engine 3606 // Stop it in the phys engine
3251 d.BodySetLinearVel(Body, 0.0f, 0.0f, _velocity.Z); 3607 d.BodySetLinearVel(Body, 0.0f, 0.0f, _velocity.Z);
3252 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); 3608 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
3253 d.BodySetForce(Body, 0f, 0f, 0f); 3609 d.BodySetForce(Body, 0f, 0f, 0f);
3254 3610
3255 if (!m_lastUpdateSent) 3611 if (!m_lastUpdateSent)
@@ -3287,71 +3643,71 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
3287 } 3643 }
3288 } 3644 }
3289 m_lastposition = l_position; 3645 m_lastposition = l_position;
3290 3646
3291 /// End UpdatePositionAndVelocity insert 3647 /// End UpdatePositionAndVelocity insert
3292 3648
3293 3649
3294 // Rotation lock ===================================== 3650 // Rotation lock =====================================
3295 if(m_rotateEnableUpdate) 3651 if (m_rotateEnableUpdate)
3296 { 3652 {
3297 // Snapshot current angles, set up Amotor(s) 3653 // Snapshot current angles, set up Amotor(s)
3298 m_rotateEnableUpdate = false; 3654 m_rotateEnableUpdate = false;
3299 m_rotateEnable = m_rotateEnableRequest; 3655 m_rotateEnable = m_rotateEnableRequest;
3300//Console.WriteLine("RotEnable {0} = {1}",m_primName, m_rotateEnable); 3656 //Console.WriteLine("RotEnable {0} = {1}",m_primName, m_rotateEnable);
3301 3657
3302 if (Amotor != IntPtr.Zero) 3658 if (Amotor != IntPtr.Zero)
3303 { 3659 {
3304 d.JointDestroy(Amotor); 3660 d.JointDestroy(Amotor);
3305 Amotor = IntPtr.Zero; 3661 Amotor = IntPtr.Zero;
3306//Console.WriteLine("Old Amotor Destroyed"); 3662 //Console.WriteLine("Old Amotor Destroyed");
3307 } 3663 }
3308 3664
3309 if (!m_rotateEnable.ApproxEquals(Vector3.One, 0.003f)) 3665 if (!m_rotateEnable.ApproxEquals(Vector3.One, 0.003f))
3310 { // not all are enabled 3666 { // not all are enabled
3311 d.Quaternion r = d.BodyGetQuaternion(Body); 3667 d.Quaternion r = d.BodyGetQuaternion(Body);
3312 Quaternion locrot = new Quaternion(r.X, r.Y, r.Z, r.W); 3668 Quaternion locrot = new Quaternion(r.X, r.Y, r.Z, r.W);
3313 // extract the axes vectors 3669 // extract the axes vectors
3314 Vector3 vX = new Vector3(1f,0f,0f); 3670 Vector3 vX = new Vector3(1f, 0f, 0f);
3315 Vector3 vY = new Vector3(0f,1f,0f); 3671 Vector3 vY = new Vector3(0f, 1f, 0f);
3316 Vector3 vZ = new Vector3(0f,0f,1f); 3672 Vector3 vZ = new Vector3(0f, 0f, 1f);
3317 vX = vX * locrot; 3673 vX = vX * locrot;
3318 vY = vY * locrot; 3674 vY = vY * locrot;
3319 vZ = vZ * locrot; 3675 vZ = vZ * locrot;
3320 // snapshot the current angle vectors 3676 // snapshot the current angle vectors
3321 m_lockX = vX; 3677 m_lockX = vX;
3322 m_lockY = vY; 3678 m_lockY = vY;
3323 m_lockZ = vZ; 3679 m_lockZ = vZ;
3324 // m_lockRot = locrot; 3680 // m_lockRot = locrot;
3325 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); 3681 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
3326 d.JointAttach(Amotor, Body, IntPtr.Zero); 3682 d.JointAttach(Amotor, Body, IntPtr.Zero);
3327 d.JointSetAMotorMode(Amotor, 0); // User mode?? 3683 d.JointSetAMotorMode(Amotor, 0); // User mode??
3328//Console.WriteLine("New Amotor Created for {0}", m_primName); 3684 //Console.WriteLine("New Amotor Created for {0}", m_primName);
3329 3685
3330 float axisnum = 3; // how many to lock 3686 float axisnum = 3; // how many to lock
3331 axisnum = (axisnum - (m_rotateEnable.X + m_rotateEnable.Y + m_rotateEnable.Z)); 3687 axisnum = (axisnum - (m_rotateEnable.X + m_rotateEnable.Y + m_rotateEnable.Z));
3332 d.JointSetAMotorNumAxes(Amotor,(int)axisnum); 3688 d.JointSetAMotorNumAxes(Amotor, (int)axisnum);
3333//Console.WriteLine("AxisNum={0}",(int)axisnum); 3689 //Console.WriteLine("AxisNum={0}",(int)axisnum);
3334 3690
3335 int i = 0; 3691 int i = 0;
3336 3692
3337 if (m_rotateEnable.X == 0) 3693 if (m_rotateEnable.X == 0)
3338 { 3694 {
3339 d.JointSetAMotorAxis(Amotor, i, 0, m_lockX.X, m_lockX.Y, m_lockX.Z); 3695 d.JointSetAMotorAxis(Amotor, i, 0, m_lockX.X, m_lockX.Y, m_lockX.Z);
3340//Console.WriteLine("AxisX {0} set to {1}", i, m_lockX); 3696 //Console.WriteLine("AxisX {0} set to {1}", i, m_lockX);
3341 i++; 3697 i++;
3342 } 3698 }
3343 3699
3344 if (m_rotateEnable.Y == 0) 3700 if (m_rotateEnable.Y == 0)
3345 { 3701 {
3346 d.JointSetAMotorAxis(Amotor, i, 0, m_lockY.X, m_lockY.Y, m_lockY.Z); 3702 d.JointSetAMotorAxis(Amotor, i, 0, m_lockY.X, m_lockY.Y, m_lockY.Z);
3347//Console.WriteLine("AxisY {0} set to {1}", i, m_lockY); 3703 //Console.WriteLine("AxisY {0} set to {1}", i, m_lockY);
3348 i++; 3704 i++;
3349 } 3705 }
3350 3706
3351 if (m_rotateEnable.Z == 0) 3707 if (m_rotateEnable.Z == 0)
3352 { 3708 {
3353 d.JointSetAMotorAxis(Amotor, i, 0, m_lockZ.X, m_lockZ.Y, m_lockZ.Z); 3709 d.JointSetAMotorAxis(Amotor, i, 0, m_lockZ.X, m_lockZ.Y, m_lockZ.Z);
3354//Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ); 3710 //Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ);
3355 i++; 3711 i++;
3356 } 3712 }
3357 3713
@@ -3362,519 +3718,519 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
3362 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f); 3718 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
3363 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); 3719 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
3364 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f); 3720 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
3365 d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 0f); 3721 d.JointSetAMotorParam(Amotor, (int)dParam.Vel, 0f);
3366 d.JointSetAMotorParam(Amotor, (int) dParam.Vel3, 0f); 3722 d.JointSetAMotorParam(Amotor, (int)dParam.Vel3, 0f);
3367 d.JointSetAMotorParam(Amotor, (int) dParam.Vel2, 0f); 3723 d.JointSetAMotorParam(Amotor, (int)dParam.Vel2, 0f);
3368 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); 3724 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f);
3369 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f); 3725 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f);
3370 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f); 3726 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f);
3371 } // else none are locked 3727 } // else none are locked
3372 } // end Rotation Update 3728 } // end Rotation Update
3373 3729
3374 3730
3375 // VEHICLE processing ========================================== 3731 // VEHICLE processing ==========================================
3376 if (m_type != Vehicle.TYPE_NONE) 3732 if (m_type != Vehicle.TYPE_NONE)
3377 { 3733 {
3378 // get body attitude 3734 // get body attitude
3379 d.Quaternion rot = d.BodyGetQuaternion(Body); 3735 d.Quaternion rot = d.BodyGetQuaternion(Body);
3380 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object 3736 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
3381 Quaternion irotq = Quaternion.Inverse(rotq); 3737 Quaternion irotq = Quaternion.Inverse(rotq);
3382 3738
3383 // VEHICLE Linear Motion 3739 // VEHICLE Linear Motion
3384 d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame 3740 d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame
3385 Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z); 3741 Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z);
3386 m_lLinObjectVel = vel_now * irotq; 3742 m_lLinObjectVel = vel_now * irotq;
3387 if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate 3743 if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate
3388 { 3744 {
3389 if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f) 3745 if (Vector3.Mag(m_lLinMotorDVel) < 1.0f)
3390 { 3746 {
3391 float decayfactor = m_linearMotorDecayTimescale/timestep; 3747 float decayfactor = m_linearMotorDecayTimescale / timestep;
3392 Vector3 decayAmount = (m_lLinMotorDVel/decayfactor); 3748 Vector3 decayAmount = (m_lLinMotorDVel / decayfactor);
3393 m_lLinMotorDVel -= decayAmount; 3749 m_lLinMotorDVel -= decayAmount;
3394 } 3750 }
3395 else 3751 else
3396 { 3752 {
3397 float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale))); 3753 float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale)));
3398 Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep; 3754 Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep;
3399 m_lLinMotorDVel -= decel; 3755 m_lLinMotorDVel -= decel;
3400 } 3756 }
3401 if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) 3757 if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
3402 { 3758 {
3403 m_lLinMotorDVel = Vector3.Zero; 3759 m_lLinMotorDVel = Vector3.Zero;
3404 } 3760 }
3405 3761
3406 /* else 3762 /* else
3407 { 3763 {
3408 if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X; 3764 if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X;
3409 if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y; 3765 if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y;
3410 if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z; 3766 if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z;
3411 } */ 3767 } */
3412 } // end linear motor decay 3768 } // end linear motor decay
3413 3769
3414 if ( (! m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) ) 3770 if ((!m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (!m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)))
3415 { 3771 {
3416 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); 3772 if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body);
3417 if (m_linearMotorTimescale < 300.0f) 3773 if (m_linearMotorTimescale < 300.0f)
3418 { 3774 {
3419 Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel; 3775 Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel;
3420 float linfactor = m_linearMotorTimescale/timestep; 3776 float linfactor = m_linearMotorTimescale / timestep;
3421 Vector3 attackAmount = (attack_error/linfactor) * 1.3f; 3777 Vector3 attackAmount = (attack_error / linfactor) * 1.3f;
3422 m_lLinObjectVel += attackAmount; 3778 m_lLinObjectVel += attackAmount;
3423 } 3779 }
3424 if (m_linearFrictionTimescale.X < 300.0f) 3780 if (m_linearFrictionTimescale.X < 300.0f)
3425 { 3781 {
3426 float fricfactor = m_linearFrictionTimescale.X / timestep; 3782 float fricfactor = m_linearFrictionTimescale.X / timestep;
3427 float fricX = m_lLinObjectVel.X / fricfactor; 3783 float fricX = m_lLinObjectVel.X / fricfactor;
3428 m_lLinObjectVel.X -= fricX; 3784 m_lLinObjectVel.X -= fricX;
3429 } 3785 }
3430 if (m_linearFrictionTimescale.Y < 300.0f) 3786 if (m_linearFrictionTimescale.Y < 300.0f)
3431 { 3787 {
3432 float fricfactor = m_linearFrictionTimescale.Y / timestep; 3788 float fricfactor = m_linearFrictionTimescale.Y / timestep;
3433 float fricY = m_lLinObjectVel.Y / fricfactor; 3789 float fricY = m_lLinObjectVel.Y / fricfactor;
3434 m_lLinObjectVel.Y -= fricY; 3790 m_lLinObjectVel.Y -= fricY;
3435 } 3791 }
3436 if (m_linearFrictionTimescale.Z < 300.0f) 3792 if (m_linearFrictionTimescale.Z < 300.0f)
3437 { 3793 {
3438 float fricfactor = m_linearFrictionTimescale.Z / timestep; 3794 float fricfactor = m_linearFrictionTimescale.Z / timestep;
3439 float fricZ = m_lLinObjectVel.Z / fricfactor; 3795 float fricZ = m_lLinObjectVel.Z / fricfactor;
3440 m_lLinObjectVel.Z -= fricZ; 3796 m_lLinObjectVel.Z -= fricZ;
3441 } 3797 }
3442 } 3798 }
3443 m_wLinObjectVel = m_lLinObjectVel * rotq; 3799 m_wLinObjectVel = m_lLinObjectVel * rotq;
3444 3800
3445 // Gravity and Buoyancy 3801 // Gravity and Buoyancy
3446 Vector3 grav = Vector3.Zero; 3802 Vector3 grav = Vector3.Zero;
3447 if(m_VehicleBuoyancy < 1.0f) 3803 if (m_VehicleBuoyancy < 1.0f)
3448 { 3804 {
3449 // There is some gravity, make a gravity force vector 3805 // There is some gravity, make a gravity force vector
3450 // that is applied after object velocity. 3806 // that is applied after object velocity.
3451 d.Mass objMass; 3807 d.Mass objMass;
3452 d.BodyGetMass(Body, out objMass); 3808 d.BodyGetMass(Body, out objMass);
3453 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; 3809 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
3454 grav.Z = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force 3810 grav.Z = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force
3455 } // else its 1.0, no gravity. 3811 } // else its 1.0, no gravity.
3456 3812
3457 // Hovering 3813 // Hovering
3458 if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) 3814 if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
3459 { 3815 {
3460 // We should hover, get the target height 3816 // We should hover, get the target height
3461 d.Vector3 pos = d.BodyGetPosition(Body); 3817 d.Vector3 pos = d.BodyGetPosition(Body);
3462 if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY) 3818 if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
3463 { 3819 {
3464 m_VhoverTargetHeight = _parent_scene.GetWaterLevel() + m_VhoverHeight; 3820 m_VhoverTargetHeight = _parent_scene.GetWaterLevel() + m_VhoverHeight;
3465 } 3821 }
3466 else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY) 3822 else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
3467 { 3823 {
3468 m_VhoverTargetHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight; 3824 m_VhoverTargetHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
3469 } 3825 }
3470 else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT) 3826 else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
3471 { 3827 {
3472 m_VhoverTargetHeight = m_VhoverHeight; 3828 m_VhoverTargetHeight = m_VhoverHeight;
3473 } 3829 }
3474 3830
3475 if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY) 3831 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
3476 { 3832 {
3477 // If body is aready heigher, use its height as target height 3833 // If body is aready heigher, use its height as target height
3478 if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z; 3834 if (pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
3479 } 3835 }
3480 3836
3481// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped 3837 // m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
3482// m_VhoverTimescale = 0f; // time to acheive height 3838 // m_VhoverTimescale = 0f; // time to acheive height
3483// timestep is time since last frame,in secs 3839 // timestep is time since last frame,in secs
3484 float herr0 = pos.Z - m_VhoverTargetHeight; 3840 float herr0 = pos.Z - m_VhoverTargetHeight;
3485 // Replace Vertical speed with correction figure if significant 3841 // Replace Vertical speed with correction figure if significant
3486 if(Math.Abs(herr0) > 0.01f ) 3842 if (Math.Abs(herr0) > 0.01f)
3487 { 3843 {
3488 //? d.Mass objMass; 3844 //? d.Mass objMass;
3489 //? d.BodyGetMass(Body, out objMass); 3845 //? d.BodyGetMass(Body, out objMass);
3490 m_wLinObjectVel.Z = - ( (herr0 * timestep * 50.0f) / m_VhoverTimescale); 3846 m_wLinObjectVel.Z = -((herr0 * timestep * 50.0f) / m_VhoverTimescale);
3491 //KF: m_VhoverEfficiency is not yet implemented 3847 //KF: m_VhoverEfficiency is not yet implemented
3492 } 3848 }
3493 else 3849 else
3494 { 3850 {
3495 m_wLinObjectVel.Z = 0f; 3851 m_wLinObjectVel.Z = 0f;
3496 } 3852 }
3497 } 3853 }
3498 else 3854 else
3499 { // not hovering 3855 { // not hovering
3500 if (m_wLinObjectVel.Z == 0f) 3856 if (m_wLinObjectVel.Z == 0f)
3501 { // Gravity rules 3857 { // Gravity rules
3502 m_wLinObjectVel.Z = vel_now.Z; 3858 m_wLinObjectVel.Z = vel_now.Z;
3503 } // else the motor has it 3859 } // else the motor has it
3504 } 3860 }
3505 linvel = m_wLinObjectVel; 3861 linvel = m_wLinObjectVel;
3506 3862
3507 // Vehicle Linear Motion done ======================================= 3863 // Vehicle Linear Motion done =======================================
3508 // Apply velocity 3864 // Apply velocity
3509 d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z); 3865 d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z);
3510 // apply gravity force 3866 // apply gravity force
3511 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); 3867 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
3512//if(frcount == 0) Console.WriteLine("Vel={0} Force={1}",linvel , grav); 3868 //if(frcount == 0) Console.WriteLine("Vel={0} Force={1}",linvel , grav);
3513 // end MoveLinear() 3869 // end MoveLinear()
3514 3870
3515 3871
3516 // MoveAngular 3872 // MoveAngular
3517 /* 3873 /*
3518 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor 3874 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
3519
3520 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
3521 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
3522 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
3523
3524 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
3525 private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body
3526 */
3527//if(frcount == 0) Console.WriteLine("MoveAngular ");
3528 3875
3529 d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body); 3876 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
3530 Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z); 3877 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
3531 angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation 3878 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
3532 3879
3533//if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel); 3880 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
3534 3881 private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body
3535 // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack. 3882 */
3536 float atk_decayfactor = 23.0f / (m_angularMotorTimescale * timestep); 3883 //if(frcount == 0) Console.WriteLine("MoveAngular ");
3537 m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor; 3884
3538 // Decay Angular Motor 2. 3885 d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body);
3539 if (m_angularMotorDecayTimescale < 300.0f) 3886 Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z);
3540 { 3887 angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation
3541 if ( Vector3.Mag(m_angularMotorDVel) < 1.0f) 3888
3542 { 3889 //if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel);
3543 float decayfactor = (m_angularMotorDecayTimescale)/timestep; 3890
3544 Vector3 decayAmount = (m_angularMotorDVel/decayfactor); 3891 // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack.
3545 m_angularMotorDVel -= decayAmount; 3892 float atk_decayfactor = 23.0f / (m_angularMotorTimescale * timestep);
3546 } 3893 m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor;
3547 else 3894 // Decay Angular Motor 2.
3548 { 3895 if (m_angularMotorDecayTimescale < 300.0f)
3549 Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale; 3896 {
3550 m_angularMotorDVel -= decel; 3897 if (Vector3.Mag(m_angularMotorDVel) < 1.0f)
3551 } 3898 {
3552 3899 float decayfactor = (m_angularMotorDecayTimescale) / timestep;
3553 if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) 3900 Vector3 decayAmount = (m_angularMotorDVel / decayfactor);
3554 { 3901 m_angularMotorDVel -= decayAmount;
3555 m_angularMotorDVel = Vector3.Zero; 3902 }
3556 } 3903 else
3557 else 3904 {
3558 { 3905 Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale;
3559 if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X; 3906 m_angularMotorDVel -= decel;
3560 if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y; 3907 }
3561 if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z; 3908
3562 } 3909 if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
3563 } // end decay angular motor 3910 {
3564//if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel); 3911 m_angularMotorDVel = Vector3.Zero;
3565 3912 }
3566//if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel); 3913 else
3567 3914 {
3568 if ( (! m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) ) 3915 if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X;
3569 { // if motor or object have motion 3916 if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y;
3570 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); 3917 if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z;
3571 3918 }
3572 if (m_angularMotorTimescale < 300.0f) 3919 } // end decay angular motor
3573 { 3920 //if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel);
3574 Vector3 attack_error = m_angularMotorDVel - angObjectVel; 3921
3575 float angfactor = m_angularMotorTimescale/timestep; 3922 //if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel);
3576 Vector3 attackAmount = (attack_error/angfactor); 3923
3577 angObjectVel += attackAmount; 3924 if ((!m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (!angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)))
3578//if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount); 3925 { // if motor or object have motion
3579//if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel); 3926 if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body);
3580 } 3927
3581 3928 if (m_angularMotorTimescale < 300.0f)
3582 angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep); 3929 {
3583 angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep); 3930 Vector3 attack_error = m_angularMotorDVel - angObjectVel;
3584 angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep); 3931 float angfactor = m_angularMotorTimescale / timestep;
3585 } // else no signif. motion 3932 Vector3 attackAmount = (attack_error / angfactor);
3586 3933 angObjectVel += attackAmount;
3587//if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel); 3934 //if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount);
3588 // Bank section tba 3935 //if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel);
3589 // Deflection section tba 3936 }
3590//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel); 3937
3591 3938 angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep);
3592 3939 angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep);
3593 /* // Rotation Axis Disables: 3940 angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep);
3594 if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f)) 3941 } // else no signif. motion
3595 { 3942
3596 if (m_angularEnable.X == 0) 3943 //if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel);
3597 angObjectVel.X = 0f; 3944 // Bank section tba
3598 if (m_angularEnable.Y == 0) 3945 // Deflection section tba
3599 angObjectVel.Y = 0f; 3946 //if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel);
3600 if (m_angularEnable.Z == 0) 3947
3601 angObjectVel.Z = 0f; 3948
3602 } 3949 /* // Rotation Axis Disables:
3603 */ 3950 if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f))
3604 angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation 3951 {
3605 3952 if (m_angularEnable.X == 0)
3606 // Vertical attractor section 3953 angObjectVel.X = 0f;
3607 Vector3 vertattr = Vector3.Zero; 3954 if (m_angularEnable.Y == 0)
3608 3955 angObjectVel.Y = 0f;
3609 if(m_verticalAttractionTimescale < 300) 3956 if (m_angularEnable.Z == 0)
3610 { 3957 angObjectVel.Z = 0f;
3611 float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep); 3958 }
3612 // make a vector pointing up 3959 */
3613 Vector3 verterr = Vector3.Zero; 3960 angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation
3614 verterr.Z = 1.0f; 3961
3615 // rotate it to Body Angle 3962 // Vertical attractor section
3616 verterr = verterr * rotq; 3963 Vector3 vertattr = Vector3.Zero;
3617 // 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. 3964
3618 // 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 3965 if (m_verticalAttractionTimescale < 300)
3619 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. 3966 {
3620 3967 float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep);
3621 if (verterr.Z < 0.0f) 3968 // make a vector pointing up
3622 { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to 3969 Vector3 verterr = Vector3.Zero;
3623 // vertical, BUT for some reason a z-rotation is imparted to the object. TBI. 3970 verterr.Z = 1.0f;
3624//Console.WriteLine("InvertFlip"); 3971 // rotate it to Body Angle
3625 verterr.X = 2.0f - verterr.X; 3972 verterr = verterr * rotq;
3626 verterr.Y = 2.0f - verterr.Y; 3973 // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
3627 } 3974 // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go
3628 verterr *= 0.5f; 3975 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
3629 // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt) 3976
3630 Vector3 xyav = angObjectVel; 3977 if (verterr.Z < 0.0f)
3631 xyav.Z = 0.0f; 3978 { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to
3632 if ((!xyav.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f)) 3979 // vertical, BUT for some reason a z-rotation is imparted to the object. TBI.
3633 { 3980 //Console.WriteLine("InvertFlip");
3634 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so 3981 verterr.X = 2.0f - verterr.X;
3635 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. 3982 verterr.Y = 2.0f - verterr.Y;
3636 vertattr.X = verterr.Y; 3983 }
3637 vertattr.Y = - verterr.X; 3984 verterr *= 0.5f;
3638 vertattr.Z = 0f; 3985 // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt)
3639//if(frcount == 0) Console.WriteLine("VAerr=" + verterr); 3986 Vector3 xyav = angObjectVel;
3640 3987 xyav.Z = 0.0f;
3641 // scaling appears better usingsquare-law 3988 if ((!xyav.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f))
3642 float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency; 3989 {
3643 float bounce = 1.0f - damped; 3990 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
3644 // 0 = crit damp, 1 = bouncy 3991 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
3645 float oavz = angObjectVel.Z; // retain z velocity 3992 vertattr.X = verterr.Y;
3646 // time-scaled correction, which sums, therefore is bouncy: 3993 vertattr.Y = -verterr.X;
3647 angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce; 3994 vertattr.Z = 0f;
3648 // damped, good @ < 90: 3995 //if(frcount == 0) Console.WriteLine("VAerr=" + verterr);
3649 angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped); 3996
3650 angObjectVel.Z = oavz; 3997 // scaling appears better usingsquare-law
3651//if(frcount == 0) Console.WriteLine("VA+"); 3998 float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
3652//Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel); 3999 float bounce = 1.0f - damped;
3653 } 4000 // 0 = crit damp, 1 = bouncy
3654 else 4001 float oavz = angObjectVel.Z; // retain z velocity
3655 { 4002 // time-scaled correction, which sums, therefore is bouncy:
3656 // else error is very small 4003 angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce;
3657 angObjectVel.X = 0f; 4004 // damped, good @ < 90:
3658 angObjectVel.Y = 0f; 4005 angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped);
3659//if(frcount == 0) Console.WriteLine("VA0"); 4006 angObjectVel.Z = oavz;
3660 } 4007 //if(frcount == 0) Console.WriteLine("VA+");
3661 } // else vertical attractor is off 4008 //Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel);
3662//if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel); 4009 }
3663 4010 else
3664 4011 {
3665 m_lastAngularVelocity = angObjectVel; 4012 // else error is very small
3666 // apply Angular Velocity to body 4013 angObjectVel.X = 0f;
3667 d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); 4014 angObjectVel.Y = 0f;
3668//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity); 4015 //if(frcount == 0) Console.WriteLine("VA0");
3669 4016 }
3670 } // end VEHICLES 4017 } // else vertical attractor is off
3671 else 4018 //if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel);
3672 { 4019
3673 // Dyamics (NON-'VEHICLES') are dealt with here ================================================================ 4020
3674 4021 m_lastAngularVelocity = angObjectVel;
3675 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 4022 // apply Angular Velocity to body
3676 4023 d.BodySetAngularVel(Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
3677 /// Dynamics Buoyancy 4024 //if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity);
3678 //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle. 4025
3679 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up 4026 } // end VEHICLES
3680 // NB Prims in ODE are no subject to global gravity 4027 else
3681 // This should only affect gravity operations 4028 {
3682 4029 // Dyamics (NON-'VEHICLES') are dealt with here ================================================================
3683 float m_mass = CalculateMass(); 4030
3684 // calculate z-force due togravity on object. 4031 if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009
3685 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass 4032
3686 if ((m_usePID) && (m_PIDTau > 0.0f)) // Dynamics llMoveToTarget. 4033 /// Dynamics Buoyancy
3687 { 4034 //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle.
3688 fz = 0; // llMoveToTarget ignores gravity. 4035 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
3689 // it also ignores mass of object, and any physical resting on it. 4036 // NB Prims in ODE are no subject to global gravity
3690 // Vector3 m_PIDTarget is where we are going 4037 // This should only affect gravity operations
3691 // float m_PIDTau is time to get there 4038
4039 float m_mass = CalculateMass();
4040 // calculate z-force due togravity on object.
4041 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass
4042 if ((m_usePID) && (m_PIDTau > 0.0f)) // Dynamics llMoveToTarget.
4043 {
4044 fz = 0; // llMoveToTarget ignores gravity.
4045 // it also ignores mass of object, and any physical resting on it.
4046 // Vector3 m_PIDTarget is where we are going
4047 // float m_PIDTau is time to get there
3692 fx = 0; 4048 fx = 0;
3693 fy = 0; 4049 fy = 0;
3694 d.Vector3 pos = d.BodyGetPosition(Body); 4050 d.Vector3 pos = d.BodyGetPosition(Body);
3695 Vector3 error = new Vector3( 4051 Vector3 error = new Vector3(
3696 (m_PIDTarget.X - pos.X), 4052 (m_PIDTarget.X - pos.X),
3697 (m_PIDTarget.Y - pos.Y), 4053 (m_PIDTarget.Y - pos.Y),
3698 (m_PIDTarget.Z - pos.Z)); 4054 (m_PIDTarget.Z - pos.Z));
3699 if (error.ApproxEquals(Vector3.Zero,0.01f)) 4055 if (error.ApproxEquals(Vector3.Zero, 0.01f))
3700 { // Very close, Jump there and quit move 4056 { // Very close, Jump there and quit move
3701 4057
3702 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); 4058 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3703 _target_velocity = Vector3.Zero; 4059 _target_velocity = Vector3.Zero;
3704 d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); 4060 d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
3705 d.BodySetForce(Body, 0f, 0f, 0f); 4061 d.BodySetForce(Body, 0f, 0f, 0f);
3706 }
3707 else
3708 {
3709 float scale = 50.0f * timestep / m_PIDTau;
3710 if ((error.ApproxEquals(Vector3.Zero,0.5f)) && (_target_velocity != Vector3.Zero))
3711 {
3712 // Nearby, quit update of velocity
3713 }
3714 else
3715 { // Far, calc damped velocity
3716 _target_velocity = error * scale;
3717 }
3718 d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
3719 } 4062 }
3720 } // end PID MoveToTarget 4063 else
3721 4064 {
3722 4065 float scale = 50.0f * timestep / m_PIDTau;
3723 /// Dynamics Hover =================================================================================== 4066 if ((error.ApproxEquals(Vector3.Zero, 0.5f)) && (_target_velocity != Vector3.Zero))
3724 // Hover PID Controller can only run if the PIDcontroller is not in use. 4067 {
3725 if (m_useHoverPID && !m_usePID) 4068 // Nearby, quit update of velocity
3726 { 4069 }
3727//Console.WriteLine("Hover " + m_primName); 4070 else
3728 4071 { // Far, calc damped velocity
3729 // If we're using the PID controller, then we have no gravity 4072 _target_velocity = error * scale;
3730 fz = (-1 * _parent_scene.gravityz) * m_mass; 4073 }
3731 4074 d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
3732 // no lock; for now it's only called from within Simulate() 4075 }
3733 4076 } // end PID MoveToTarget
3734 // If the PID Controller isn't active then we set our force 4077
3735 // calculating base velocity to the current position 4078
3736 4079 /// Dynamics Hover ===================================================================================
3737 if ((m_PIDTau < 1)) 4080 // Hover PID Controller can only run if the PIDcontroller is not in use.
3738 { 4081 if (m_useHoverPID && !m_usePID)
3739 PID_G = PID_G / m_PIDTau; 4082 {
3740 } 4083 //Console.WriteLine("Hover " + m_primName);
3741 4084
3742 if ((PID_G - m_PIDTau) <= 0) 4085 // If we're using the PID controller, then we have no gravity
3743 { 4086 fz = (-1 * _parent_scene.gravityz) * m_mass;
3744 PID_G = m_PIDTau + 1; 4087
3745 } 4088 // no lock; for now it's only called from within Simulate()
3746 4089
3747 4090 // If the PID Controller isn't active then we set our force
3748 // Where are we, and where are we headed? 4091 // calculating base velocity to the current position
3749 d.Vector3 pos = d.BodyGetPosition(Body); 4092
3750// d.Vector3 vel = d.BodyGetLinearVel(Body); 4093 if ((m_PIDTau < 1))
3751 4094 {
3752 4095 PID_G = PID_G / m_PIDTau;
3753 // Non-Vehicles have a limited set of Hover options. 4096 }
3754 // determine what our target height really is based on HoverType 4097
3755 switch (m_PIDHoverType) 4098 if ((PID_G - m_PIDTau) <= 0)
3756 { 4099 {
3757 case PIDHoverType.Ground: 4100 PID_G = m_PIDTau + 1;
3758 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); 4101 }
3759 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; 4102
3760 break; 4103
3761 case PIDHoverType.GroundAndWater: 4104 // Where are we, and where are we headed?
3762 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); 4105 d.Vector3 pos = d.BodyGetPosition(Body);
3763 m_waterHeight = _parent_scene.GetWaterLevel(); 4106 // d.Vector3 vel = d.BodyGetLinearVel(Body);
3764 if (m_groundHeight > m_waterHeight) 4107
3765 { 4108
3766 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; 4109 // Non-Vehicles have a limited set of Hover options.
3767 } 4110 // determine what our target height really is based on HoverType
3768 else 4111 switch (m_PIDHoverType)
3769 { 4112 {
3770 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; 4113 case PIDHoverType.Ground:
3771 } 4114 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3772 break; 4115 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3773 4116 break;
3774 } // end switch (m_PIDHoverType) 4117 case PIDHoverType.GroundAndWater:
3775 4118 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3776 4119 m_waterHeight = _parent_scene.GetWaterLevel();
3777 _target_velocity = 4120 if (m_groundHeight > m_waterHeight)
4121 {
4122 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
4123 }
4124 else
4125 {
4126 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
4127 }
4128 break;
4129
4130 } // end switch (m_PIDHoverType)
4131
4132
4133 _target_velocity =
3778 new Vector3(0.0f, 0.0f, 4134 new Vector3(0.0f, 0.0f,
3779 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) 4135 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
3780 ); 4136 );
3781 4137
3782 // if velocity is zero, use position control; otherwise, velocity control 4138 // if velocity is zero, use position control; otherwise, velocity control
3783 4139
3784 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) 4140 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
3785 { 4141 {
3786 // keep track of where we stopped. No more slippin' & slidin' 4142 // keep track of where we stopped. No more slippin' & slidin'
3787 4143
3788 // We only want to deactivate the PID Controller if we think we want to have our surrogate 4144 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3789 // react to the physics scene by moving it's position. 4145 // react to the physics scene by moving it's position.
3790 // Avatar to Avatar collisions 4146 // Avatar to Avatar collisions
3791 // Prim to avatar collisions 4147 // Prim to avatar collisions
3792 d.Vector3 dlinvel = vel; 4148 d.Vector3 dlinvel = vel;
3793 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); 4149 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
3794 d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z); 4150 d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z);
3795 d.BodyAddForce(Body, 0, 0, fz); 4151 d.BodyAddForce(Body, 0, 0, fz);
3796 //KF this prevents furthur motions return; 4152 //KF this prevents furthur motions return;
3797 } 4153 }
3798 else 4154 else
3799 { 4155 {
3800 _zeroFlag = false; 4156 _zeroFlag = false;
3801 4157
3802 // We're flying and colliding with something 4158 // We're flying and colliding with something
3803 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); 4159 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
3804 } 4160 }
3805 } // end m_useHoverPID && !m_usePID 4161 } // end m_useHoverPID && !m_usePID
3806 4162
3807 4163
3808 /// Dynamics Apply Forces =================================================================================== 4164 /// Dynamics Apply Forces ===================================================================================
3809 fx *= m_mass; 4165 fx *= m_mass;
3810 fy *= m_mass; 4166 fy *= m_mass;
3811 //fz *= m_mass; 4167 //fz *= m_mass;
3812 fx += m_force.X; 4168 fx += m_force.X;
3813 fy += m_force.Y; 4169 fy += m_force.Y;
3814 fz += m_force.Z; 4170 fz += m_force.Z;
3815 4171
3816 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); 4172 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
3817 if (fx != 0 || fy != 0 || fz != 0) 4173 if (fx != 0 || fy != 0 || fz != 0)
3818 { 4174 {
3819 //m_taintdisable = true; 4175 //m_taintdisable = true;
3820 //base.RaiseOutOfBounds(Position); 4176 //base.RaiseOutOfBounds(Position);
3821 //d.BodySetLinearVel(Body, fx, fy, 0f); 4177 //d.BodySetLinearVel(Body, fx, fy, 0f);
3822 if (!d.BodyIsEnabled(Body)) 4178 if (!d.BodyIsEnabled(Body))
3823 { 4179 {
3824 // A physical body at rest on a surface will auto-disable after a while, 4180 // A physical body at rest on a surface will auto-disable after a while,
3825 // this appears to re-enable it incase the surface it is upon vanishes, 4181 // this appears to re-enable it incase the surface it is upon vanishes,
3826 // and the body should fall again. 4182 // and the body should fall again.
3827 d.BodySetLinearVel(Body, 0f, 0f, 0f); 4183 d.BodySetLinearVel(Body, 0f, 0f, 0f);
3828 d.BodySetForce(Body, 0f, 0f, 0f); 4184 d.BodySetForce(Body, 0f, 0f, 0f);
3829 enableBodySoft(); 4185 enableBodySoft();
3830 } 4186 }
3831 4187
3832 // 35x10 = 350n times the mass per second applied maximum. 4188 // 35x10 = 350n times the mass per second applied maximum.
3833 float nmax = 35f * m_mass; 4189 float nmax = 35f * m_mass;
3834 float nmin = -35f * m_mass; 4190 float nmin = -35f * m_mass;
3835 4191
3836 4192
3837 if (fx > nmax) 4193 if (fx > nmax)
3838 fx = nmax; 4194 fx = nmax;
3839 if (fx < nmin) 4195 if (fx < nmin)
3840 fx = nmin; 4196 fx = nmin;
3841 if (fy > nmax) 4197 if (fy > nmax)
3842 fy = nmax; 4198 fy = nmax;
3843 if (fy < nmin) 4199 if (fy < nmin)
3844 fy = nmin; 4200 fy = nmin;
3845 d.BodyAddForce(Body, fx, fy, fz); 4201 d.BodyAddForce(Body, fx, fy, fz);
3846 } // end apply forces 4202 } // end apply forces
3847 } // end Vehicle/Dynamics 4203 } // end Vehicle/Dynamics
3848 4204
3849 /// RotLookAt / LookAt ================================================================================= 4205 /// RotLookAt / LookAt =================================================================================
3850 if (m_useAPID) 4206 if (m_useAPID)
3851 { 4207 {
3852 // RotLookAt, apparently overrides all other rotation sources. Inputs: 4208 // RotLookAt, apparently overrides all other rotation sources. Inputs:
3853 // Quaternion m_APIDTarget 4209 // Quaternion m_APIDTarget
3854 // float m_APIDStrength // From SL experiments, this is the time to get there 4210 // float m_APIDStrength // From SL experiments, this is the time to get there
3855 // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly 4211 // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
3856 // Also in SL the mass of the object has no effect on time to get there. 4212 // Also in SL the mass of the object has no effect on time to get there.
3857 // Factors: 4213 // Factors:
3858 // get present body rotation 4214 // get present body rotation
3859 float limit = 1.0f; 4215 float limit = 1.0f;
3860 float rscaler = 50f; // adjusts rotation damping time 4216 float rscaler = 50f; // adjusts rotation damping time
3861 float lscaler = 10f; // adjusts linear damping time in llLookAt 4217 float lscaler = 10f; // adjusts linear damping time in llLookAt
3862 float RLAservo = 0f; 4218 float RLAservo = 0f;
3863 Vector3 diff_axis; 4219 Vector3 diff_axis;
3864 float diff_angle; 4220 float diff_angle;
3865 d.Quaternion rot = d.BodyGetQuaternion(Body); // prim present rotation 4221 d.Quaternion rot = d.BodyGetQuaternion(Body); // prim present rotation
3866 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); 4222 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
3867 Quaternion rtarget = new Quaternion(); 4223 Quaternion rtarget = new Quaternion();
3868 4224
3869 if(m_APIDTarget.W == -99.9f) 4225 if (m_APIDTarget.W == -99.9f)
3870 { 4226 {
3871 // this is really a llLookAt(), x,y,z is the target vector 4227 // this is really a llLookAt(), x,y,z is the target vector
3872 Vector3 target = new Vector3(m_APIDTarget.X, m_APIDTarget.Y, m_APIDTarget.Z); 4228 Vector3 target = new Vector3(m_APIDTarget.X, m_APIDTarget.Y, m_APIDTarget.Z);
3873 Vector3 ospin = new Vector3(1.0f, 0.0f, 0.0f) * rotq; 4229 Vector3 ospin = new Vector3(1.0f, 0.0f, 0.0f) * rotq;
3874 Vector3 error = new Vector3(0.0f, 0.0f, 0.0f); 4230 Vector3 error = new Vector3(0.0f, 0.0f, 0.0f);
3875 float twopi = 2.0f * (float)Math.PI; 4231 float twopi = 2.0f * (float)Math.PI;
3876 Vector3 dir = target - _position; 4232 Vector3 dir = target - _position;
3877 dir.Normalize(); 4233 dir.Normalize();
3878 float tzrot = (float)Math.Atan2(dir.Y, dir.X); 4234 float tzrot = (float)Math.Atan2(dir.Y, dir.X);
3879 float txy = (float)Math.Sqrt((dir.X * dir.X) + (dir.Y * dir.Y)); 4235 float txy = (float)Math.Sqrt((dir.X * dir.X) + (dir.Y * dir.Y));
3880 float terot = (float)Math.Atan2(dir.Z, txy); 4236 float terot = (float)Math.Atan2(dir.Z, txy);
@@ -3882,63 +4238,63 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
3882 float oxy = (float)Math.Sqrt((ospin.X * ospin.X) + (ospin.Y * ospin.Y)); 4238 float oxy = (float)Math.Sqrt((ospin.X * ospin.X) + (ospin.Y * ospin.Y));
3883 float oerot = (float)Math.Atan2(ospin.Z, oxy); 4239 float oerot = (float)Math.Atan2(ospin.Z, oxy);
3884 float ra = 2.0f * ((rotq.W * rotq.X) + (rotq.Y * rotq.Z)); 4240 float ra = 2.0f * ((rotq.W * rotq.X) + (rotq.Y * rotq.Z));
3885 float rb = 1.0f - 2.0f * ((rotq.Y * rotq.Y)+(rotq.X * rotq.X)); 4241 float rb = 1.0f - 2.0f * ((rotq.Y * rotq.Y) + (rotq.X * rotq.X));
3886 float roll = (float)Math.Atan2(ra, rb); 4242 float roll = (float)Math.Atan2(ra, rb);
3887 float errorz = tzrot - ozrot; 4243 float errorz = tzrot - ozrot;
3888 if(errorz > (float)Math.PI) errorz -= twopi; 4244 if (errorz > (float)Math.PI) errorz -= twopi;
3889 else if(errorz < -(float)Math.PI) errorz += twopi; 4245 else if (errorz < -(float)Math.PI) errorz += twopi;
3890 float errory = oerot - terot; 4246 float errory = oerot - terot;
3891 if(errory > (float)Math.PI) errory -= twopi; 4247 if (errory > (float)Math.PI) errory -= twopi;
3892 else if(errory < -(float)Math.PI) errory += twopi; 4248 else if (errory < -(float)Math.PI) errory += twopi;
3893 diff_angle = Math.Abs(errorz) + Math.Abs(errory) + Math.Abs(roll); 4249 diff_angle = Math.Abs(errorz) + Math.Abs(errory) + Math.Abs(roll);
3894 if(diff_angle > 0.01f * m_APIDdamper) 4250 if (diff_angle > 0.01f * m_APIDdamper)
3895 { 4251 {
3896 m_APIDdamper = 1.0f; 4252 m_APIDdamper = 1.0f;
3897 RLAservo = timestep / m_APIDStrength * rscaler; 4253 RLAservo = timestep / m_APIDStrength * rscaler;
3898 errorz *= RLAservo; 4254 errorz *= RLAservo;
3899 errory *= RLAservo; 4255 errory *= RLAservo;
3900 error.X = -roll * 8.0f; 4256 error.X = -roll * 8.0f;
3901 error.Y = errory; 4257 error.Y = errory;
3902 error.Z = errorz; 4258 error.Z = errorz;
3903 error *= rotq; 4259 error *= rotq;
3904 d.BodySetAngularVel (Body, error.X, error.Y, error.Z); 4260 d.BodySetAngularVel(Body, error.X, error.Y, error.Z);
3905 } 4261 }
3906 else 4262 else
3907 { 4263 {
3908 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); 4264 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
3909 m_APIDdamper = 2.0f; 4265 m_APIDdamper = 2.0f;
3910 } 4266 }
3911 } 4267 }
3912 else 4268 else
3913 { 4269 {
3914 // this is a llRotLookAt() 4270 // this is a llRotLookAt()
3915 rtarget = m_APIDTarget; 4271 rtarget = m_APIDTarget;
3916 4272
3917 Quaternion rot_diff = Quaternion.Inverse(rotq) * rtarget; // difference to desired rot 4273 Quaternion rot_diff = Quaternion.Inverse(rotq) * rtarget; // difference to desired rot
3918 rot_diff.GetAxisAngle(out diff_axis, out diff_angle); // convert to axis to point at & error angle 4274 rot_diff.GetAxisAngle(out diff_axis, out diff_angle); // convert to axis to point at & error angle
3919//if(frcount == 0) Console.WriteLine("axis {0} angle {1}",diff_axis * 57.3f, diff_angle); 4275 //if(frcount == 0) Console.WriteLine("axis {0} angle {1}",diff_axis * 57.3f, diff_angle);
3920 4276
3921 // diff_axis.Normalize(); it already is! 4277 // diff_axis.Normalize(); it already is!
3922 if(diff_angle > 0.01f * m_APIDdamper) // diff_angle is always +ve // if there is enough error 4278 if (diff_angle > 0.01f * m_APIDdamper) // diff_angle is always +ve // if there is enough error
3923 { 4279 {
3924 m_APIDdamper = 1.0f; 4280 m_APIDdamper = 1.0f;
3925 Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z); 4281 Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
3926 rotforce = rotforce * rotq; 4282 rotforce = rotforce * rotq;
3927 if(diff_angle > limit) diff_angle = limit; // cap the rotate rate 4283 if (diff_angle > limit) diff_angle = limit; // cap the rotate rate
3928 RLAservo = timestep / m_APIDStrength * lscaler; 4284 RLAservo = timestep / m_APIDStrength * lscaler;
3929 rotforce = rotforce * RLAservo * diff_angle ; 4285 rotforce = rotforce * RLAservo * diff_angle;
3930 d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z); 4286 d.BodySetAngularVel(Body, rotforce.X, rotforce.Y, rotforce.Z);
3931//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); 4287 //Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo);
3932 } 4288 }
3933 else 4289 else
3934 { // close enough 4290 { // close enough
3935 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); 4291 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
3936 m_APIDdamper = 2.0f; 4292 m_APIDdamper = 2.0f;
3937 } 4293 }
3938 } // end llLookAt/llRotLookAt 4294 } // end llLookAt/llRotLookAt
3939//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle); 4295 //if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle);
3940 } // end m_useAPID 4296 } // end m_useAPID
3941 } // end root prims 4297 } // end root prims
3942 } // end Move() 4298 } // end Move()
3943 } // end class 4299 } // end class
3944} 4300}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
index cf7c1d7..b5b30ee 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
@@ -1736,6 +1736,23 @@ namespace OpenSim.Region.Physics.OdePlugin
1736 return newPrim; 1736 return newPrim;
1737 } 1737 }
1738 1738
1739 private PhysicsActor AddPrim(String name, Vector3 position, PhysicsActor parent,
1740 PrimitiveBaseShape pbs, uint localid, byte[] sdata)
1741 {
1742 Vector3 pos = position;
1743
1744 OdePrim newPrim;
1745 lock (OdeLock)
1746 {
1747 newPrim = new OdePrim(name, this, pos, parent, pbs, ode, localid, sdata);
1748 lock (_prims)
1749 _prims.Add(newPrim);
1750 }
1751
1752 return newPrim;
1753 }
1754
1755
1739 public void addActivePrim(OdePrim activatePrim) 1756 public void addActivePrim(OdePrim activatePrim)
1740 { 1757 {
1741 // adds active prim.. (ones that should be iterated over in collisions_optimized 1758 // adds active prim.. (ones that should be iterated over in collisions_optimized
@@ -1762,6 +1779,17 @@ namespace OpenSim.Region.Physics.OdePlugin
1762 return result; 1779 return result;
1763 } 1780 }
1764 1781
1782 public override PhysicsActor AddPrimShape(string primName, PhysicsActor parent, PrimitiveBaseShape pbs, Vector3 position,
1783 uint localid, byte[] sdata)
1784 {
1785 PhysicsActor result;
1786
1787 result = AddPrim(primName, position, parent,
1788 pbs, localid, sdata);
1789
1790 return result;
1791 }
1792
1765 public override float TimeDilation 1793 public override float TimeDilation
1766 { 1794 {
1767 get { return m_timeDilation; } 1795 get { return m_timeDilation; }
@@ -3412,15 +3440,15 @@ namespace OpenSim.Region.Physics.OdePlugin
3412 3440
3413 int regionsize = (int) Constants.RegionSize; // visible region size eg. 256(M) 3441 int regionsize = (int) Constants.RegionSize; // visible region size eg. 256(M)
3414 3442
3415 int heightmapWidth = regionsize + 2; // ODE map size 257 x 257 (Meters) (1 extra 3443 int heightmapWidth = regionsize + 2; // ODE map size 258 x 258 (Meters) (1 extra each side)
3416 int heightmapHeight = regionsize + 2; 3444 int heightmapHeight = regionsize + 2;
3417 3445
3418 int heightmapWidthSamples = (int)regionsize + 2; // Sample file size, 258 x 258 samples 3446 int heightmapWidthSamples = (int)regionsize + 3; // to have 258m we need 259 samples
3419 int heightmapHeightSamples = (int)regionsize + 2; 3447 int heightmapHeightSamples = (int)regionsize + 3;
3420 3448
3421 // Array of height samples for ODE 3449 // Array of height samples for ODE
3422 float[] _heightmap; 3450 float[] _heightmap;
3423 _heightmap = new float[(heightmapWidthSamples * heightmapHeightSamples)]; // loaded samples 258 x 258 3451 _heightmap = new float[(heightmapWidthSamples * heightmapHeightSamples)]; // loaded samples 259 x 259
3424 3452
3425 // Other ODE parameters 3453 // Other ODE parameters
3426 const float scale = 1.0f; 3454 const float scale = 1.0f;
diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs b/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs
new file mode 100644
index 0000000..e7e7bb3
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs
@@ -0,0 +1,353 @@
1/* Ubit 2012
2 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
3 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
4 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
5 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
6 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
7 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
8 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
9 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
10 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
11 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
12*/
13
14// no endian conversion. So can't be use to pass information around diferent cpus with diferent endian
15
16using System;
17using System.IO;
18using OpenMetaverse;
19
20namespace OpenSim.Region.Physics.OdePlugin
21{
22
23 unsafe public class wstreamer
24 {
25 byte[] buf;
26 int index;
27 byte* src;
28
29 public wstreamer()
30 {
31 buf = new byte[1024];
32 index = 0;
33 }
34 public wstreamer(int size)
35 {
36 buf = new byte[size];
37 index = 0;
38 }
39
40 public byte[] close()
41 {
42 byte[] data = new byte[index];
43 Buffer.BlockCopy(buf, 0, data, 0, index);
44 return data;
45 }
46
47 public void Seek(int pos)
48 {
49 index = pos;
50 }
51
52 public void Seekrel(int pos)
53 {
54 index += pos;
55 }
56
57 public void Wbyte(byte value)
58 {
59 buf[index++] = value;
60 }
61 public void Wshort(short value)
62 {
63 src = (byte*)&value;
64 buf[index++] = *src++;
65 buf[index++] = *src;
66 }
67 public void Wushort(ushort value)
68 {
69 src = (byte*)&value;
70 buf[index++] = *src++;
71 buf[index++] = *src;
72 }
73 public void Wint(int value)
74 {
75 src = (byte*)&value;
76 buf[index++] = *src++;
77 buf[index++] = *src++;
78 buf[index++] = *src++;
79 buf[index++] = *src;
80 }
81 public void Wuint(uint value)
82 {
83 src = (byte*)&value;
84 buf[index++] = *src++;
85 buf[index++] = *src++;
86 buf[index++] = *src++;
87 buf[index++] = *src;
88 }
89 public void Wlong(long value)
90 {
91 src = (byte*)&value;
92 buf[index++] = *src++;
93 buf[index++] = *src++;
94 buf[index++] = *src++;
95 buf[index++] = *src++;
96 buf[index++] = *src++;
97 buf[index++] = *src++;
98 buf[index++] = *src++;
99 buf[index++] = *src;
100 }
101 public void Wulong(ulong value)
102 {
103 src = (byte*)&value;
104 buf[index++] = *src++;
105 buf[index++] = *src++;
106 buf[index++] = *src++;
107 buf[index++] = *src++;
108 buf[index++] = *src++;
109 buf[index++] = *src++;
110 buf[index++] = *src++;
111 buf[index++] = *src;
112 }
113
114 public void Wfloat(float value)
115 {
116 src = (byte*)&value;
117 buf[index++] = *src++;
118 buf[index++] = *src++;
119 buf[index++] = *src++;
120 buf[index++] = *src;
121 }
122
123 public void Wdouble(double value)
124 {
125 src = (byte*)&value;
126 buf[index++] = *src++;
127 buf[index++] = *src++;
128 buf[index++] = *src++;
129 buf[index++] = *src++;
130 buf[index++] = *src++;
131 buf[index++] = *src++;
132 buf[index++] = *src++;
133 buf[index++] = *src;
134 }
135
136 public void Wvector3(Vector3 value)
137 {
138 src = (byte*)&value.X;
139 buf[index++] = *src++;
140 buf[index++] = *src++;
141 buf[index++] = *src++;
142 buf[index++] = *src;
143 src = (byte*)&value.Y; // it may have padding ??
144 buf[index++] = *src++;
145 buf[index++] = *src++;
146 buf[index++] = *src++;
147 buf[index++] = *src;
148 src = (byte*)&value.Z;
149 buf[index++] = *src++;
150 buf[index++] = *src++;
151 buf[index++] = *src++;
152 buf[index++] = *src;
153 }
154 public void Wquat(Quaternion value)
155 {
156 src = (byte*)&value.X;
157 buf[index++] = *src++;
158 buf[index++] = *src++;
159 buf[index++] = *src++;
160 buf[index++] = *src;
161 src = (byte*)&value.Y; // it may have padding ??
162 buf[index++] = *src++;
163 buf[index++] = *src++;
164 buf[index++] = *src++;
165 buf[index++] = *src;
166 src = (byte*)&value.Z;
167 buf[index++] = *src++;
168 buf[index++] = *src++;
169 buf[index++] = *src++;
170 buf[index++] = *src;
171 src = (byte*)&value.W;
172 buf[index++] = *src++;
173 buf[index++] = *src++;
174 buf[index++] = *src++;
175 buf[index++] = *src;
176 }
177 }
178
179 unsafe public class rstreamer
180 {
181 private byte[] rbuf;
182 private int ptr;
183 private byte* dst;
184
185 public rstreamer(byte[] data)
186 {
187 rbuf = data;
188 ptr = 0;
189 }
190
191 public void close()
192 {
193 }
194
195 public void Seek(int pos)
196 {
197 ptr = pos;
198 }
199
200 public void Seekrel(int pos)
201 {
202 ptr += pos;
203 }
204
205 public byte Rbyte()
206 {
207 return (byte)rbuf[ptr++];
208 }
209
210 public short Rshort()
211 {
212 short v;
213 dst = (byte*)&v;
214 *dst++ = rbuf[ptr++];
215 *dst = rbuf[ptr++];
216 return v;
217 }
218 public ushort Rushort()
219 {
220 ushort v;
221 dst = (byte*)&v;
222 *dst++ = rbuf[ptr++];
223 *dst = rbuf[ptr++];
224 return v;
225 }
226 public int Rint()
227 {
228 int v;
229 dst = (byte*)&v;
230 *dst++ = rbuf[ptr++];
231 *dst++ = rbuf[ptr++];
232 *dst++ = rbuf[ptr++];
233 *dst = rbuf[ptr++];
234 return v;
235 }
236 public uint Ruint()
237 {
238 uint v;
239 dst = (byte*)&v;
240 *dst++ = rbuf[ptr++];
241 *dst++ = rbuf[ptr++];
242 *dst++ = rbuf[ptr++];
243 *dst = rbuf[ptr++];
244 return v;
245 }
246 public long Rlong()
247 {
248 long v;
249 dst = (byte*)&v;
250 *dst++ = rbuf[ptr++];
251 *dst++ = rbuf[ptr++];
252 *dst++ = rbuf[ptr++];
253 *dst++ = rbuf[ptr++];
254 *dst++ = rbuf[ptr++];
255 *dst++ = rbuf[ptr++];
256 *dst++ = rbuf[ptr++];
257 *dst = rbuf[ptr++];
258 return v;
259 }
260 public ulong Rulong()
261 {
262 ulong v;
263 dst = (byte*)&v;
264 *dst++ = rbuf[ptr++];
265 *dst++ = rbuf[ptr++];
266 *dst++ = rbuf[ptr++];
267 *dst++ = rbuf[ptr++];
268 *dst++ = rbuf[ptr++];
269 *dst++ = rbuf[ptr++];
270 *dst++ = rbuf[ptr++];
271 *dst = rbuf[ptr++];
272 return v;
273 }
274 public float Rfloat()
275 {
276 float v;
277 dst = (byte*)&v;
278 *dst++ = rbuf[ptr++];
279 *dst++ = rbuf[ptr++];
280 *dst++ = rbuf[ptr++];
281 *dst = rbuf[ptr++];
282 return v;
283 }
284
285 public double Rdouble()
286 {
287 double v;
288 dst = (byte*)&v;
289 *dst++ = rbuf[ptr++];
290 *dst++ = rbuf[ptr++];
291 *dst++ = rbuf[ptr++];
292 *dst++ = rbuf[ptr++];
293 *dst++ = rbuf[ptr++];
294 *dst++ = rbuf[ptr++];
295 *dst++ = rbuf[ptr++];
296 *dst = rbuf[ptr++];
297 return v;
298 }
299
300 public Vector3 Rvector3()
301 {
302 Vector3 v;
303 dst = (byte*)&v.X;
304 *dst++ = rbuf[ptr++];
305 *dst++ = rbuf[ptr++];
306 *dst++ = rbuf[ptr++];
307 *dst = rbuf[ptr++];
308
309 dst = (byte*)&v.Y;
310 *dst++ = rbuf[ptr++];
311 *dst++ = rbuf[ptr++];
312 *dst++ = rbuf[ptr++];
313 *dst = rbuf[ptr++];
314
315 dst = (byte*)&v.Z;
316 *dst++ = rbuf[ptr++];
317 *dst++ = rbuf[ptr++];
318 *dst++ = rbuf[ptr++];
319 *dst = rbuf[ptr++];
320 return v;
321 }
322
323 public Quaternion Rquat()
324 {
325 Quaternion v;
326 dst = (byte*)&v.X;
327 *dst++ = rbuf[ptr++];
328 *dst++ = rbuf[ptr++];
329 *dst++ = rbuf[ptr++];
330 *dst = rbuf[ptr++];
331
332 dst = (byte*)&v.Y;
333 *dst++ = rbuf[ptr++];
334 *dst++ = rbuf[ptr++];
335 *dst++ = rbuf[ptr++];
336 *dst = rbuf[ptr++];
337
338 dst = (byte*)&v.Z;
339 *dst++ = rbuf[ptr++];
340 *dst++ = rbuf[ptr++];
341 *dst++ = rbuf[ptr++];
342 *dst = rbuf[ptr++];
343
344 dst = (byte*)&v.W;
345 *dst++ = rbuf[ptr++];
346 *dst++ = rbuf[ptr++];
347 *dst++ = rbuf[ptr++];
348 *dst = rbuf[ptr++];
349
350 return v;
351 }
352 }
353}
generated by cgit v1.1 at 2024-11-18 17:42:35 +0000