aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
diff options
context:
space:
mode:
authorUbitUmarov2012-02-17 21:09:00 +0000
committerUbitUmarov2012-02-17 21:09:00 +0000
commit7d77ccc6593c6c3ac9f66e2e593dfb8cc719cd04 (patch)
tree22b49c5533ed40f76d861f77c1b15a821606756a /OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
parentNow if chode prim.cs detects out of bounds it requests a update and blocks mo... (diff)
downloadopensim-SC-7d77ccc6593c6c3ac9f66e2e593dfb8cc719cd04.zip
opensim-SC-7d77ccc6593c6c3ac9f66e2e593dfb8cc719cd04.tar.gz
opensim-SC-7d77ccc6593c6c3ac9f66e2e593dfb8cc719cd04.tar.bz2
opensim-SC-7d77ccc6593c6c3ac9f66e2e593dfb8cc719cd04.tar.xz
Added simple binary serializer/deserializer to chODE. 100% untested and most like still broken
Diffstat (limited to '')
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs2368
1 files changed, 1333 insertions, 1035 deletions
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
index a68dcb7..6e28bfa 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -27,6 +27,7 @@
27 */ 27 */
28 28
29using System; 29using System;
30using System.IO;
30using System.Collections.Generic; 31using System.Collections.Generic;
31using System.Reflection; 32using System.Reflection;
32using System.Runtime.InteropServices; 33using System.Runtime.InteropServices;
@@ -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,470 @@ 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
253
254 255
256 SerialControl m_taintserial = null;
257
258 public override byte[] Serialize(bool PhysIsRunning)
259 {
260 SerialControl sc = new SerialControl();
261
262 lock (sc.alock)
263 {
264 if (PhysIsRunning)
265 {
266 m_taintserial = sc;
267
268 if (!Monitor.Wait(sc.alock, 1000))
269 {
270 m_log.Error("[chOde] prim data serialization timed out");
271 m_taintserial = null;
272 return new byte[0];
273 }
274 }
275 else
276 DoSerialize(sc);
277 }
278
279 return sc.data;
280 }
281
282 public void DoSerialize(SerialControl sc)
283 {
284 wstreamer st = new wstreamer();
285 Vector3 vtmp;
286
287 ushort version = 2;
288 if (!BitConverter.IsLittleEndian)
289 version |= 1;
290 st.Wushort(version); //version lower bit codes endian type for future use
291
292 // compact booleans in a ushort
293 ushort flags = 0;
294
295 if (m_isphysical) // this should be true for now
296 flags |= 1;
297 if (m_isSelected)
298 flags |= 2;
299 if (m_isVolumeDetect)
300 flags |= 4;
301 if (m_disabled)
302 flags |= 8;
303 if (m_collidesWater)
304 flags |= 16;
305 if (m_collidesLand)
306 flags |= 32;
307 if (m_usePID)
308 flags |= 64;
309 if (m_useAPID)
310 flags |= 128;
311 if (m_useHoverPID)
312 flags |= 256;
313 if (m_throttleUpdates)
314 flags |= 512;
315
316 st.Wushort(flags);
317
318 st.Wvector3(_size);
319 st.Wint(m_material);
320 st.Wfloat(m_density);
321 st.Wfloat(0); // future gravity mod V3
322 st.Wfloat(0); // future friction V3
323 st.Wfloat(0); // future bounce V3
324
325// st.Wuint((uint)m_collisionCategories);
326// st.Wuint((uint)m_collisionFlags);
327
328 if (_parent == null)
329 {
330 st.Wvector3(_position); // ??
331 st.Wquat(_orientation);
332 }
333 else // for childs save offsets
334 {
335 Quaternion to;
336 Quaternion ipo = Quaternion.Inverse(_parent.Orientation);
337
338 if (m_isphysical && prim_geom != IntPtr.Zero)
339 {
340 d.Vector3 dvt;
341 d.GeomCopyPosition(prim_geom, out dvt);
342
343 vtmp.X = dvt.X;
344 vtmp.Y = dvt.Y;
345 vtmp.Z = dvt.Z;
346
347 d.Quaternion dqt;
348 d.GeomCopyQuaternion(prim_geom, out dqt);
349
350 to.X = dqt.X;
351 to.Y = dqt.Y;
352 to.Z = dqt.Z;
353 to.W = dqt.W; // rotation in world
354 }
355 else
356 {
357 vtmp = _position;
358 to = _orientation;
359 }
360
361 vtmp -= _parent.Position; // offset in world
362 vtmp *= ipo; // offset in local
363 st.Wvector3(vtmp);
364
365 ipo *= to; // own rotation
366 st.Wquat(ipo);
367 }
368
369 st.Wvector3(_velocity);
370 st.Wvector3(m_rotationalVelocity);
371 st.Wvector3(_acceleration);
372 st.Wvector3(m_rotateEnable);
373
374 vtmp = Vector3.Zero;
375 for (int i = 0; i < m_forcelist.Count; i++)
376 {
377
378 vtmp += (m_forcelist[i] * 100);
379 }
380
381 st.Wvector3(vtmp); // force acc
382
383 vtmp = Vector3.Zero;
384 for (int i = 0; i < m_angularforcelist.Count; i++)
385 {
386 vtmp += (m_angularforcelist[i] * 100);
387 }
388
389 st.Wvector3(vtmp); // angular force acc
390
391 st.Wvector3(m_PIDTarget);
392 st.Wfloat(m_PIDTau);
393 st.Wfloat(PID_D);
394 st.Wfloat(PID_G);
395 st.Wquat(m_APIDTarget);
396 st.Wfloat(m_APIDStrength);
397 st.Wfloat(m_APIDDamping);
398 st.Wfloat(m_APIDdamper);
399
400 st.Wint((int)m_PIDHoverType);
401 st.Wfloat(m_PIDHoverHeight);
402 st.Wfloat(m_PIDHoverTau);
403 st.Wfloat(m_targetHoverHeight);
404
405 st.Wfloat(m_groundHeight);
406 st.Wfloat(m_waterHeight);
407
408 st.Wfloat(m_buoyancy);
409
410 // this must be last since type none ends stream
411 if (m_type == Vehicle.TYPE_NONE)
412 st.Wint((int)Vehicle.TYPE_NONE);
413 else
414 {
415 st.Wint((int)m_type);
416
417 st.Wquat(Quaternion.Identity); //m_referenceFrame
255 418
419 st.Wint((int)m_flags);
256 420
421 st.Wvector3(m_linearMotorDirection);
422 st.Wfloat(
423 (float)Math.Sqrt(m_lLinMotorDVel.LengthSquared() / m_linearMotorDirection.LengthSquared()));
424
425 st.Wvector3(m_linearFrictionTimescale);
426 st.Wfloat(m_linearMotorDecayTimescale);
427 st.Wfloat(m_linearMotorTimescale);
428 st.Wvector3(new Vector3(0, 0, 0)); //m_linearMotorOffset);
429
430 st.Wvector3(m_angularMotorDirection);
431 st.Wfloat((float)Math.Sqrt(m_angularMotorDVel.LengthSquared() / m_angularMotorDirection.LengthSquared()));
432
433 st.Wvector3(m_angularFrictionTimescale);
434 st.Wfloat(m_angularMotorDecayTimescale);
435 st.Wfloat(m_angularMotorTimescale);
436
437 st.Wfloat(0); //m_linearDeflectionEfficiency);
438 st.Wfloat(1000); //m_linearDeflectionTimescale);
439
440 st.Wfloat(0); //m_angularDeflectionEfficiency);
441 st.Wfloat(120); //m_angularDeflectionTimescale);
442
443 st.Wfloat(0); // m_bankingEfficiency);
444 st.Wfloat(0); //m_bankingMix);
445 st.Wfloat(1000); //m_bankingTimescale);
446
447 st.Wfloat(m_VhoverHeight);
448 st.Wfloat(0.5f); //m_VhoverEfficiency);
449 st.Wfloat(m_VhoverTimescale);
450
451 st.Wfloat(m_VehicleBuoyancy);
452
453 st.Wfloat(m_verticalAttractionEfficiency);
454 st.Wfloat(m_verticalAttractionTimescale);
455 }
456 sc.data = st.close();
457 m_taintserial = null;
458 Monitor.PulseAll(sc.alock);
459 }
460
461 public bool DeSerialize(byte[] data)
462 {
463 rstreamer st = new rstreamer(data);
464
465 int version =st.Rushort(); //version
466
467 // merge booleans in a ushort
468 ushort flags = st.Rushort();
469 if ((flags & 1) != 0)
470 m_isphysical = true;
471 if ((flags & 2) != 0)
472 m_taintselected = true;
473 if ((flags & 4) != 0)
474 m_isVolumeDetect = true;
475 if ((flags & 8) != 0)
476 m_taintdisable = true;
477 if ((flags & 16) != 0)
478 m_taintCollidesWater = true;
479 if ((flags & 32) != 0)
480 m_collidesLand = true;
481 if ((flags & 64) != 0)
482 m_usePID = true;
483 if ((flags & 128) != 0)
484 m_useAPID = true;
485 if ((flags & 256) != 0)
486 m_useHoverPID = true;
487 if ((flags & 512) != 0)
488 m_throttleUpdates = true;
489
490 _size = st.Rvector3();
491 m_taintsize = _size;
492
493 m_material= st.Rint();
494 m_density = st.Rfloat();
495 st.Rfloat(); // future gravity mod V3
496 st.Rfloat(); // future friction V3
497 st.Rfloat(); // future bounce V3
498
499// m_collisionCategories = (CollisionCategories)st.Ruint();
500// m_collisionFlags = (CollisionCategories) st.Ruint();
501
502 if (m_taintparent == null)
503 {
504 st.Rvector3(); // ignore old position sop/sog as to tell the new one
505 m_taintrot = st.Rquat(); //
506 _orientation = m_taintrot;
507 }
508 else
509 {
510 m_taintrot = _parent.Orientation;
511 m_taintposition = st.Rvector3(); // ??
512 _position = m_taintposition;
513
514 m_taintposition *= m_taintrot;
515 m_taintposition += _parent.Position;
516
517 m_taintrot *= st.Rquat(); //
518 _orientation = m_taintrot;
519 }
520
521 m_taintVelocity = st.Rvector3();
522 m_rotationalVelocity = st.Rvector3();
523
524 _acceleration = st.Rvector3();
525 m_rotateEnableRequest = st.Rvector3();
526 m_rotateEnableUpdate = true;
527
528 Vector3 vtmp;
529
530 vtmp = st.Rvector3(); // forces acc
531 m_forcelist.Add(vtmp);
532 m_taintforce = true;
533
534 vtmp = st.Rvector3(); // angular forces acc
535 m_angularforcelist.Add(vtmp);
536 m_taintaddangularforce = true;
537
538 m_PIDTarget = st.Rvector3();
539 m_PIDTau = st.Rfloat();
540 PID_D = st.Rfloat();
541 PID_G = st.Rfloat();
542
543 m_APIDTarget = st.Rquat();
544 m_APIDStrength = st.Rfloat();
545 m_APIDDamping = st.Rfloat();
546 m_APIDdamper = st.Rfloat();
547
548 m_PIDHoverType = (PIDHoverType) st.Rint();
549 m_PIDHoverHeight = st.Rfloat();
550 m_PIDHoverTau = st.Rfloat();
551 m_targetHoverHeight = st.Rfloat();
552
553 m_groundHeight = st.Rfloat();
554 m_waterHeight = st.Rfloat();
555
556 m_buoyancy = st.Rfloat();
557
558
559 // this must be last since type none ends stream
560
561 m_type = (Vehicle) st.Rint();
562
563 if (m_type != Vehicle.TYPE_NONE)
564 {
565 float ftmp;
566
567 st.Rquat(); //m_referenceFrame
568
569 m_flags = (VehicleFlag) st.Rint();
570
571 m_linearMotorDirection = st.Rvector3();
572
573 ftmp = st.Rfloat();
574 m_lLinMotorDVel = m_linearMotorDirection * ftmp;
575
576 m_linearFrictionTimescale = st.Rvector3();
577 m_linearMotorDecayTimescale = st.Rfloat();
578 m_linearMotorTimescale = st.Rfloat();
579 st.Rvector3(); //m_linearMotorOffset);
580
581 m_angularMotorDirection = st.Rvector3();
582 ftmp = st.Rfloat();
583 m_angularMotorDVel = m_angularMotorDirection * ftmp;
584
585 m_angularFrictionTimescale = st.Rvector3();
586 m_angularMotorDecayTimescale = st.Rfloat();
587 m_angularMotorTimescale = st.Rfloat();
588
589 st.Rfloat(); //m_linearDeflectionEfficiency);
590 st.Rfloat(); //m_linearDeflectionTimescale);
591
592 st.Rfloat(); //m_angularDeflectionEfficiency);
593 st.Rfloat(); //m_angularDeflectionTimescale);
594
595 st.Rfloat(); // m_bankingEfficiency);
596 st.Rfloat(); //m_bankingMix);
597 st.Rfloat(); //m_bankingTimescale);
598
599 m_VhoverHeight = st.Rfloat();
600 st.Rfloat(); //m_VhoverEfficiency);
601 m_VhoverTimescale = st.Rfloat();
602
603 m_VehicleBuoyancy = st.Rfloat();
604
605 m_verticalAttractionEfficiency = st.Rfloat();
606 m_verticalAttractionTimescale = st.Rfloat();
607 }
608 st.close();
609 return true;
610 }
611
612 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, PhysicsActor parent,
613 PrimitiveBaseShape pbs, CollisionLocker dode, uint localid, byte[] sdata)
614 {
615 m_localID = localid;
616 ode = dode;
617
618 if (parent == null)
619 {
620 m_taintparent = null;
621
622 if (!pos.IsFinite())
623 {
624 pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
625 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
626 m_log.Warn("[PHYSICS]: Got nonFinite Object create Position");
627 }
628
629 _position = pos;
630 m_taintposition = pos;
631 }
632 else
633 m_taintparent = parent;
634
635 body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
636
637 prim_geom = IntPtr.Zero;
638
639 _mesh = null;
640 m_meshfailed = false;
641 _pbs = pbs;
642
643 _parent_scene = parent_scene;
644 m_targetSpace = (IntPtr)0;
645
646 if(sdata != null && sdata.Length > 1)
647 DeSerialize(sdata);
648
649 if (m_isphysical)
650 m_targetSpace = _parent_scene.space;
651
652 m_primName = primName;
653 m_taintserial = null;
654 m_taintadd = true;
655 _parent_scene.AddPhysicsActorTaint(this);
656 // don't do .add() here; old geoms get recycled with the same hash
657 }
257 658
258 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, 659 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
259 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode, uint localid) 660 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode, uint localid)
@@ -266,7 +667,7 @@ namespace OpenSim.Region.Physics.OdePlugin
266 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); 667 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"); 668 m_log.Warn("[PHYSICS]: Got nonFinite Object create Position");
268 } 669 }
269 670
270 _position = pos; 671 _position = pos;
271 m_taintposition = pos; 672 m_taintposition = pos;
272 PID_D = parent_scene.bodyPIDD; 673 PID_D = parent_scene.bodyPIDD;
@@ -275,9 +676,8 @@ namespace OpenSim.Region.Physics.OdePlugin
275 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; 676 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
276 body_autodisable_frames = parent_scene.bodyFramesAutoDisable; 677 body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
277 678
278
279 prim_geom = IntPtr.Zero; 679 prim_geom = IntPtr.Zero;
280// prev_geom = IntPtr.Zero; 680 // prev_geom = IntPtr.Zero;
281 681
282 if (!pos.IsFinite()) 682 if (!pos.IsFinite())
283 { 683 {
@@ -306,7 +706,7 @@ namespace OpenSim.Region.Physics.OdePlugin
306 _parent_scene = parent_scene; 706 _parent_scene = parent_scene;
307 m_targetSpace = (IntPtr)0; 707 m_targetSpace = (IntPtr)0;
308 708
309// if (pos.Z < 0) 709 // if (pos.Z < 0)
310 if (pos.Z < parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y)) 710 if (pos.Z < parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y))
311 m_isphysical = false; 711 m_isphysical = false;
312 else 712 else
@@ -317,6 +717,8 @@ namespace OpenSim.Region.Physics.OdePlugin
317 if (m_isphysical) 717 if (m_isphysical)
318 m_targetSpace = _parent_scene.space; 718 m_targetSpace = _parent_scene.space;
319 } 719 }
720
721 m_taintserial = null;
320 m_primName = primName; 722 m_primName = primName;
321 m_taintadd = true; 723 m_taintadd = true;
322 _parent_scene.AddPhysicsActorTaint(this); 724 _parent_scene.AddPhysicsActorTaint(this);
@@ -325,7 +727,7 @@ namespace OpenSim.Region.Physics.OdePlugin
325 727
326 public override int PhysicsActorType 728 public override int PhysicsActorType
327 { 729 {
328 get { return (int) ActorTypes.Prim; } 730 get { return (int)ActorTypes.Prim; }
329 set { return; } 731 set { return; }
330 } 732 }
331 733
@@ -337,9 +739,11 @@ namespace OpenSim.Region.Physics.OdePlugin
337 739
338 public override uint LocalID 740 public override uint LocalID
339 { 741 {
340 set { 742 set
743 {
341 //m_log.Info("[PHYSICS]: Setting TrackerID: " + value); 744 //m_log.Info("[PHYSICS]: Setting TrackerID: " + value);
342 m_localID = value; } 745 m_localID = value;
746 }
343 } 747 }
344 748
345 public override bool Grabbed 749 public override bool Grabbed
@@ -349,9 +753,10 @@ namespace OpenSim.Region.Physics.OdePlugin
349 753
350 public override bool Selected 754 public override bool Selected
351 { 755 {
352 set { 756 set
353 757 {
354//Console.WriteLine("Sel {0} {1} {2}", m_primName, value, m_isphysical); 758
759 //Console.WriteLine("Sel {0} {1} {2}", m_primName, value, m_isphysical);
355 // This only makes the object not collidable if the object 760 // This only makes the object not collidable if the object
356 // is physical or the object is modified somehow *IN THE FUTURE* 761 // is physical or the object is modified somehow *IN THE FUTURE*
357 // without this, if an avatar selects prim, they can walk right 762 // without this, if an avatar selects prim, they can walk right
@@ -367,21 +772,21 @@ namespace OpenSim.Region.Physics.OdePlugin
367 m_taintselected = value; 772 m_taintselected = value;
368 m_isSelected = value; 773 m_isSelected = value;
369 } 774 }
370 if(m_isSelected) disableBodySoft(); 775 if (m_isSelected) disableBodySoft();
371 } 776 }
372 } 777 }
373 778
374 public override bool IsPhysical 779 public override bool IsPhysical
375 { 780 {
376 get { return m_isphysical; } 781 get { return m_isphysical; }
377 set 782 set
378 { 783 {
379 m_isphysical = value; 784 m_isphysical = value;
380 if (!m_isphysical) 785 if (!m_isphysical)
381 { // Zero the remembered last velocity 786 { // Zero the remembered last velocity
382 m_lastVelocity = Vector3.Zero; 787 m_lastVelocity = Vector3.Zero;
383 if (m_type != Vehicle.TYPE_NONE) Halt(); 788 if (m_type != Vehicle.TYPE_NONE) Halt();
384 } 789 }
385 } 790 }
386 } 791 }
387 792
@@ -430,7 +835,9 @@ namespace OpenSim.Region.Physics.OdePlugin
430 { 835 {
431 get { return _position; } 836 get { return _position; }
432 837
433 set { _position = value; 838 set
839 {
840 _position = value;
434 //m_log.Info("[PHYSICS]: " + _position.ToString()); 841 //m_log.Info("[PHYSICS]: " + _position.ToString());
435 } 842 }
436 } 843 }
@@ -481,29 +888,29 @@ namespace OpenSim.Region.Physics.OdePlugin
481 888
482 public override void VehicleFloatParam(int param, float value) 889 public override void VehicleFloatParam(int param, float value)
483 { 890 {
484 ProcessFloatVehicleParam((Vehicle) param, value); 891 ProcessFloatVehicleParam((Vehicle)param, value);
485 } 892 }
486 893
487 public override void VehicleVectorParam(int param, Vector3 value) 894 public override void VehicleVectorParam(int param, Vector3 value)
488 { 895 {
489 ProcessVectorVehicleParam((Vehicle) param, value); 896 ProcessVectorVehicleParam((Vehicle)param, value);
490 } 897 }
491 898
492 public override void VehicleRotationParam(int param, Quaternion rotation) 899 public override void VehicleRotationParam(int param, Quaternion rotation)
493 { 900 {
494 ProcessRotationVehicleParam((Vehicle) param, rotation); 901 ProcessRotationVehicleParam((Vehicle)param, rotation);
495 } 902 }
496 903
497 public override void VehicleFlags(int param, bool remove) 904 public override void VehicleFlags(int param, bool remove)
498 { 905 {
499 ProcessVehicleFlags(param, remove); 906 ProcessVehicleFlags(param, remove);
500 } 907 }
501 908
502 public override void SetVolumeDetect(int param) 909 public override void SetVolumeDetect(int param)
503 { 910 {
504 lock (_parent_scene.OdeLock) 911 lock (_parent_scene.OdeLock)
505 { 912 {
506 m_isVolumeDetect = (param!=0); 913 m_isVolumeDetect = (param != 0);
507 } 914 }
508 } 915 }
509 916
@@ -536,9 +943,9 @@ namespace OpenSim.Region.Physics.OdePlugin
536 return Vector3.Zero; 943 return Vector3.Zero;
537 944
538 Vector3 returnVelocity = Vector3.Zero; 945 Vector3 returnVelocity = Vector3.Zero;
539 returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2; 946 returnVelocity.X = (m_lastVelocity.X + _velocity.X) / 2;
540 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2; 947 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) / 2;
541 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2; 948 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) / 2;
542 return returnVelocity; 949 return returnVelocity;
543 } 950 }
544 set 951 set
@@ -546,8 +953,8 @@ namespace OpenSim.Region.Physics.OdePlugin
546 if (value.IsFinite()) 953 if (value.IsFinite())
547 { 954 {
548 _velocity = value; 955 _velocity = value;
549 if (_velocity.ApproxEquals(Vector3.Zero,0.001f)) 956 if (_velocity.ApproxEquals(Vector3.Zero, 0.001f))
550 _acceleration = Vector3.Zero; 957 _acceleration = Vector3.Zero;
551 958
552 m_taintVelocity = value; 959 m_taintVelocity = value;
553 _parent_scene.AddPhysicsActorTaint(this); 960 _parent_scene.AddPhysicsActorTaint(this);
@@ -604,17 +1011,18 @@ namespace OpenSim.Region.Physics.OdePlugin
604 if (QuaternionIsFinite(value)) 1011 if (QuaternionIsFinite(value))
605 { 1012 {
606 _orientation = value; 1013 _orientation = value;
607 } 1014 }
608 else 1015 else
609 m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object"); 1016 m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object");
610 1017
611 } 1018 }
612 } 1019 }
613 1020
614 1021
615 public override bool FloatOnWater 1022 public override bool FloatOnWater
616 { 1023 {
617 set { 1024 set
1025 {
618 m_taintCollidesWater = value; 1026 m_taintCollidesWater = value;
619 _parent_scene.AddPhysicsActorTaint(this); 1027 _parent_scene.AddPhysicsActorTaint(this);
620 } 1028 }
@@ -624,8 +1032,8 @@ namespace OpenSim.Region.Physics.OdePlugin
624 { 1032 {
625 } 1033 }
626 1034
627 public override Vector3 PIDTarget 1035 public override Vector3 PIDTarget
628 { 1036 {
629 set 1037 set
630 { 1038 {
631 if (value.IsFinite()) 1039 if (value.IsFinite())
@@ -634,16 +1042,16 @@ namespace OpenSim.Region.Physics.OdePlugin
634 } 1042 }
635 else 1043 else
636 m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object"); 1044 m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object");
637 } 1045 }
638 } 1046 }
639 public override bool PIDActive { set { m_usePID = value; } } 1047 public override bool PIDActive { set { m_usePID = value; } }
640 public override float PIDTau { set { m_PIDTau = value; } } 1048 public override float PIDTau { set { m_PIDTau = value; } }
641 1049
642 // For RotLookAt 1050 // For RotLookAt
643 public override Quaternion APIDTarget { set { m_APIDTarget = value; } } 1051 public override Quaternion APIDTarget { set { m_APIDTarget = value; } }
644 public override bool APIDActive { set { m_useAPID = value; } } 1052 public override bool APIDActive { set { m_useAPID = value; } }
645 public override float APIDStrength { set { m_APIDStrength = value; } } 1053 public override float APIDStrength { set { m_APIDStrength = value; } }
646 public override float APIDDamping { set { m_APIDDamping = value; } } 1054 public override float APIDDamping { set { m_APIDDamping = value; } }
647 1055
648 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } 1056 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
649 public override bool PIDHoverActive { set { m_useHoverPID = value; } } 1057 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
@@ -665,13 +1073,13 @@ namespace OpenSim.Region.Physics.OdePlugin
665 1073
666 public override Vector3 Acceleration // client updates read data via here 1074 public override Vector3 Acceleration // client updates read data via here
667 { 1075 {
668 get 1076 get
669 { 1077 {
670 if (_zeroFlag) 1078 if (_zeroFlag)
671 { 1079 {
672 return Vector3.Zero; 1080 return Vector3.Zero;
673 } 1081 }
674 return _acceleration; 1082 return _acceleration;
675 } 1083 }
676 set { _acceleration = value; } 1084 set { _acceleration = value; }
677 } 1085 }
@@ -752,18 +1160,18 @@ namespace OpenSim.Region.Physics.OdePlugin
752 base.RequestPhysicsterseUpdate(); 1160 base.RequestPhysicsterseUpdate();
753 m_outofBounds = false; 1161 m_outofBounds = false;
754 } 1162 }
755/* 1163 /*
756 int tmp = Interlocked.Increment(ref m_crossingfailures); 1164 int tmp = Interlocked.Increment(ref m_crossingfailures);
757 if (tmp > _parent_scene.geomCrossingFailuresBeforeOutofbounds) 1165 if (tmp > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
758 { 1166 {
759 base.RaiseOutOfBounds(_position); 1167 base.RaiseOutOfBounds(_position);
760 return; 1168 return;
761 } 1169 }
762 else if (tmp == _parent_scene.geomCrossingFailuresBeforeOutofbounds) 1170 else if (tmp == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
763 { 1171 {
764 m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName); 1172 m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);
765 } 1173 }
766 */ 1174 */
767 } 1175 }
768 1176
769 public override float Buoyancy 1177 public override float Buoyancy
@@ -806,26 +1214,26 @@ namespace OpenSim.Region.Physics.OdePlugin
806 1214
807 public void SetGeom(IntPtr geom) 1215 public void SetGeom(IntPtr geom)
808 { 1216 {
809 if(prim_geom != IntPtr.Zero) 1217 if (prim_geom != IntPtr.Zero)
810 { 1218 {
811 // Remove any old entries 1219 // Remove any old entries
812//string tPA; 1220 //string tPA;
813//_parent_scene.geom_name_map.TryGetValue(prim_geom, out tPA); 1221 //_parent_scene.geom_name_map.TryGetValue(prim_geom, out tPA);
814//Console.WriteLine("**** Remove {0}", tPA); 1222 //Console.WriteLine("**** Remove {0}", tPA);
815 if(_parent_scene.geom_name_map.ContainsKey(prim_geom)) _parent_scene.geom_name_map.Remove(prim_geom); 1223 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); 1224 if (_parent_scene.actor_name_map.ContainsKey(prim_geom)) _parent_scene.actor_name_map.Remove(prim_geom);
817 d.GeomDestroy(prim_geom); 1225 d.GeomDestroy(prim_geom);
818 } 1226 }
819 1227
820 prim_geom = geom; 1228 prim_geom = geom;
821//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName); 1229 //Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName);
822 if (prim_geom != IntPtr.Zero) 1230 if (prim_geom != IntPtr.Zero)
823 { 1231 {
824 _parent_scene.geom_name_map[prim_geom] = this.m_primName; 1232 _parent_scene.geom_name_map[prim_geom] = this.m_primName;
825 _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this; 1233 _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
826 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 1234 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
827 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 1235 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); 1236 //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 } 1237 }
830 1238
831 if (childPrim) 1239 if (childPrim)
@@ -833,7 +1241,7 @@ namespace OpenSim.Region.Physics.OdePlugin
833 if (_parent != null && _parent is OdePrim) 1241 if (_parent != null && _parent is OdePrim)
834 { 1242 {
835 OdePrim parent = (OdePrim)_parent; 1243 OdePrim parent = (OdePrim)_parent;
836//Console.WriteLine("SetGeom calls ChildSetGeom"); 1244 //Console.WriteLine("SetGeom calls ChildSetGeom");
837 parent.ChildSetGeom(this); 1245 parent.ChildSetGeom(this);
838 } 1246 }
839 } 1247 }
@@ -848,7 +1256,7 @@ namespace OpenSim.Region.Physics.OdePlugin
848 { 1256 {
849 d.BodyEnable(Body); 1257 d.BodyEnable(Body);
850 if (m_type != Vehicle.TYPE_NONE) 1258 if (m_type != Vehicle.TYPE_NONE)
851 Enable(Body, _parent_scene); 1259 Enable(Body, _parent_scene);
852 } 1260 }
853 1261
854 m_disabled = false; 1262 m_disabled = false;
@@ -892,9 +1300,9 @@ namespace OpenSim.Region.Physics.OdePlugin
892 1300
893 d.BodySetAutoDisableFlag(Body, true); 1301 d.BodySetAutoDisableFlag(Body, true);
894 d.BodySetAutoDisableSteps(Body, body_autodisable_frames); 1302 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
895 1303
896 // disconnect from world gravity so we can apply buoyancy 1304 // disconnect from world gravity so we can apply buoyancy
897 d.BodySetGravityMode (Body, false); 1305 d.BodySetGravityMode(Body, false);
898 1306
899 m_interpenetrationcount = 0; 1307 m_interpenetrationcount = 0;
900 m_collisionscore = 0; 1308 m_collisionscore = 0;
@@ -918,19 +1326,19 @@ namespace OpenSim.Region.Physics.OdePlugin
918 1326
919 float returnMass = 0; 1327 float returnMass = 0;
920 float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; 1328 float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
921 float hollowVolume = hollowAmount * hollowAmount; 1329 float hollowVolume = hollowAmount * hollowAmount;
922 1330
923 switch (_pbs.ProfileShape) 1331 switch (_pbs.ProfileShape)
924 { 1332 {
925 case ProfileShape.Square: 1333 case ProfileShape.Square:
926 // default box 1334 // default box
927 1335
928 if (_pbs.PathCurve == (byte)Extrusion.Straight) 1336 if (_pbs.PathCurve == (byte)Extrusion.Straight)
929 { 1337 {
930 if (hollowAmount > 0.0) 1338 if (hollowAmount > 0.0)
931 { 1339 {
932 switch (_pbs.HollowShape) 1340 switch (_pbs.HollowShape)
933 { 1341 {
934 case HollowShape.Square: 1342 case HollowShape.Square:
935 case HollowShape.Same: 1343 case HollowShape.Same:
936 break; 1344 break;
@@ -948,31 +1356,31 @@ namespace OpenSim.Region.Physics.OdePlugin
948 default: 1356 default:
949 hollowVolume = 0; 1357 hollowVolume = 0;
950 break; 1358 break;
951 }
952 volume *= (1.0f - hollowVolume);
953 } 1359 }
1360 volume *= (1.0f - hollowVolume);
954 } 1361 }
1362 }
955 1363
956 else if (_pbs.PathCurve == (byte)Extrusion.Curve1) 1364 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
957 { 1365 {
958 //a tube 1366 //a tube
959 1367
960 volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); 1368 volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
961 tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY); 1369 tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY);
962 volume -= volume*tmp*tmp; 1370 volume -= volume * tmp * tmp;
963 1371
964 if (hollowAmount > 0.0) 1372 if (hollowAmount > 0.0)
965 { 1373 {
966 hollowVolume *= hollowAmount; 1374 hollowVolume *= hollowAmount;
967 1375
968 switch (_pbs.HollowShape) 1376 switch (_pbs.HollowShape)
969 { 1377 {
970 case HollowShape.Square: 1378 case HollowShape.Square:
971 case HollowShape.Same: 1379 case HollowShape.Same:
972 break; 1380 break;
973 1381
974 case HollowShape.Circle: 1382 case HollowShape.Circle:
975 hollowVolume *= 0.78539816339f;; 1383 hollowVolume *= 0.78539816339f; ;
976 break; 1384 break;
977 1385
978 case HollowShape.Triangle: 1386 case HollowShape.Triangle:
@@ -981,23 +1389,23 @@ namespace OpenSim.Region.Physics.OdePlugin
981 default: 1389 default:
982 hollowVolume = 0; 1390 hollowVolume = 0;
983 break; 1391 break;
984 }
985 volume *= (1.0f - hollowVolume);
986 } 1392 }
1393 volume *= (1.0f - hollowVolume);
987 } 1394 }
1395 }
988 1396
989 break; 1397 break;
990 1398
991 case ProfileShape.Circle: 1399 case ProfileShape.Circle:
992 1400
993 if (_pbs.PathCurve == (byte)Extrusion.Straight) 1401 if (_pbs.PathCurve == (byte)Extrusion.Straight)
994 { 1402 {
995 volume *= 0.78539816339f; // elipse base 1403 volume *= 0.78539816339f; // elipse base
996 1404
997 if (hollowAmount > 0.0) 1405 if (hollowAmount > 0.0)
998 { 1406 {
999 switch (_pbs.HollowShape) 1407 switch (_pbs.HollowShape)
1000 { 1408 {
1001 case HollowShape.Same: 1409 case HollowShape.Same:
1002 case HollowShape.Circle: 1410 case HollowShape.Circle:
1003 break; 1411 break;
@@ -1013,25 +1421,25 @@ namespace OpenSim.Region.Physics.OdePlugin
1013 default: 1421 default:
1014 hollowVolume = 0; 1422 hollowVolume = 0;
1015 break; 1423 break;
1016 }
1017 volume *= (1.0f - hollowVolume);
1018 } 1424 }
1425 volume *= (1.0f - hollowVolume);
1019 } 1426 }
1427 }
1020 1428
1021 else if (_pbs.PathCurve == (byte)Extrusion.Curve1) 1429 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1022 { 1430 {
1023 volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); 1431 volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
1024 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); 1432 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
1025 volume *= (1.0f - tmp * tmp); 1433 volume *= (1.0f - tmp * tmp);
1026 1434
1027 if (hollowAmount > 0.0) 1435 if (hollowAmount > 0.0)
1028 { 1436 {
1029 1437
1030 // calculate the hollow volume by it's shape compared to the prim shape 1438 // calculate the hollow volume by it's shape compared to the prim shape
1031 hollowVolume *= hollowAmount; 1439 hollowVolume *= hollowAmount;
1032 1440
1033 switch (_pbs.HollowShape) 1441 switch (_pbs.HollowShape)
1034 { 1442 {
1035 case HollowShape.Same: 1443 case HollowShape.Same:
1036 case HollowShape.Circle: 1444 case HollowShape.Circle:
1037 break; 1445 break;
@@ -1047,31 +1455,31 @@ namespace OpenSim.Region.Physics.OdePlugin
1047 default: 1455 default:
1048 hollowVolume = 0; 1456 hollowVolume = 0;
1049 break; 1457 break;
1050 }
1051 volume *= (1.0f - hollowVolume);
1052 } 1458 }
1459 volume *= (1.0f - hollowVolume);
1053 } 1460 }
1461 }
1054 break; 1462 break;
1055 1463
1056 case ProfileShape.HalfCircle: 1464 case ProfileShape.HalfCircle:
1057 if (_pbs.PathCurve == (byte)Extrusion.Curve1) 1465 if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1058 { 1466 {
1059 volume *= 0.52359877559829887307710723054658f; 1467 volume *= 0.52359877559829887307710723054658f;
1060 } 1468 }
1061 break; 1469 break;
1062 1470
1063 case ProfileShape.EquilateralTriangle: 1471 case ProfileShape.EquilateralTriangle:
1064 1472
1065 if (_pbs.PathCurve == (byte)Extrusion.Straight) 1473 if (_pbs.PathCurve == (byte)Extrusion.Straight)
1066 { 1474 {
1067 volume *= 0.32475953f; 1475 volume *= 0.32475953f;
1068 1476
1069 if (hollowAmount > 0.0) 1477 if (hollowAmount > 0.0)
1070 { 1478 {
1071 1479
1072 // calculate the hollow volume by it's shape compared to the prim shape 1480 // calculate the hollow volume by it's shape compared to the prim shape
1073 switch (_pbs.HollowShape) 1481 switch (_pbs.HollowShape)
1074 { 1482 {
1075 case HollowShape.Same: 1483 case HollowShape.Same:
1076 case HollowShape.Triangle: 1484 case HollowShape.Triangle:
1077 hollowVolume *= .25f; 1485 hollowVolume *= .25f;
@@ -1091,24 +1499,24 @@ namespace OpenSim.Region.Physics.OdePlugin
1091 default: 1499 default:
1092 hollowVolume = 0; 1500 hollowVolume = 0;
1093 break; 1501 break;
1094 }
1095 volume *= (1.0f - hollowVolume);
1096 } 1502 }
1503 volume *= (1.0f - hollowVolume);
1097 } 1504 }
1505 }
1098 else if (_pbs.PathCurve == (byte)Extrusion.Curve1) 1506 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1099 { 1507 {
1100 volume *= 0.32475953f; 1508 volume *= 0.32475953f;
1101 volume *= 0.01f * (float)(200 - _pbs.PathScaleX); 1509 volume *= 0.01f * (float)(200 - _pbs.PathScaleX);
1102 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); 1510 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
1103 volume *= (1.0f - tmp * tmp); 1511 volume *= (1.0f - tmp * tmp);
1104 1512
1105 if (hollowAmount > 0.0) 1513 if (hollowAmount > 0.0)
1106 { 1514 {
1107 1515
1108 hollowVolume *= hollowAmount; 1516 hollowVolume *= hollowAmount;
1109 1517
1110 switch (_pbs.HollowShape) 1518 switch (_pbs.HollowShape)
1111 { 1519 {
1112 case HollowShape.Same: 1520 case HollowShape.Same:
1113 case HollowShape.Triangle: 1521 case HollowShape.Triangle:
1114 hollowVolume *= .25f; 1522 hollowVolume *= .25f;
@@ -1126,15 +1534,15 @@ namespace OpenSim.Region.Physics.OdePlugin
1126 default: 1534 default:
1127 hollowVolume = 0; 1535 hollowVolume = 0;
1128 break; 1536 break;
1129 }
1130 volume *= (1.0f - hollowVolume);
1131 } 1537 }
1538 volume *= (1.0f - hollowVolume);
1132 } 1539 }
1133 break; 1540 }
1541 break;
1134 1542
1135 default: 1543 default:
1136 break; 1544 break;
1137 } 1545 }
1138 1546
1139 1547
1140 1548
@@ -1148,7 +1556,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1148 float profileEnd; 1556 float profileEnd;
1149 1557
1150 if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) 1558 if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible)
1151 { 1559 {
1152 taperX1 = _pbs.PathScaleX * 0.01f; 1560 taperX1 = _pbs.PathScaleX * 0.01f;
1153 if (taperX1 > 1.0f) 1561 if (taperX1 > 1.0f)
1154 taperX1 = 2.0f - taperX1; 1562 taperX1 = 2.0f - taperX1;
@@ -1158,9 +1566,9 @@ namespace OpenSim.Region.Physics.OdePlugin
1158 if (taperY1 > 1.0f) 1566 if (taperY1 > 1.0f)
1159 taperY1 = 2.0f - taperY1; 1567 taperY1 = 2.0f - taperY1;
1160 taperY = 1.0f - taperY1; 1568 taperY = 1.0f - taperY1;
1161 } 1569 }
1162 else 1570 else
1163 { 1571 {
1164 taperX = _pbs.PathTaperX * 0.01f; 1572 taperX = _pbs.PathTaperX * 0.01f;
1165 if (taperX < 0.0f) 1573 if (taperX < 0.0f)
1166 taperX = -taperX; 1574 taperX = -taperX;
@@ -1168,10 +1576,10 @@ namespace OpenSim.Region.Physics.OdePlugin
1168 1576
1169 taperY = _pbs.PathTaperY * 0.01f; 1577 taperY = _pbs.PathTaperY * 0.01f;
1170 if (taperY < 0.0f) 1578 if (taperY < 0.0f)
1171 taperY = -taperY; 1579 taperY = -taperY;
1172 taperY1 = 1.0f - taperY; 1580 taperY1 = 1.0f - taperY;
1173 1581
1174 } 1582 }
1175 1583
1176 1584
1177 volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); 1585 volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY);
@@ -1180,7 +1588,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1180 pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; 1588 pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f;
1181 volume *= (pathEnd - pathBegin); 1589 volume *= (pathEnd - pathBegin);
1182 1590
1183// this is crude aproximation 1591 // this is crude aproximation
1184 profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; 1592 profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f;
1185 profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; 1593 profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f;
1186 volume *= (profileEnd - profileBegin); 1594 volume *= (profileEnd - profileBegin);
@@ -1189,8 +1597,8 @@ namespace OpenSim.Region.Physics.OdePlugin
1189 1597
1190 if (returnMass <= 0) 1598 if (returnMass <= 0)
1191 returnMass = 0.0001f;//ckrinke: Mass must be greater then zero. 1599 returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
1192// else if (returnMass > _parent_scene.maximumMassObject) 1600 // else if (returnMass > _parent_scene.maximumMassObject)
1193// returnMass = _parent_scene.maximumMassObject; 1601 // returnMass = _parent_scene.maximumMassObject;
1194 1602
1195 1603
1196 1604
@@ -1230,7 +1638,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1230 1638
1231 public void setMass() 1639 public void setMass()
1232 { 1640 {
1233 if (Body != (IntPtr) 0) 1641 if (Body != (IntPtr)0)
1234 { 1642 {
1235 float newmass = CalculateMass(); 1643 float newmass = CalculateMass();
1236 1644
@@ -1260,7 +1668,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1260 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 1668 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1261 } 1669 }
1262 1670
1263 1671
1264 d.BodyDestroy(Body); 1672 d.BodyDestroy(Body);
1265 lock (childrenPrim) 1673 lock (childrenPrim)
1266 { 1674 {
@@ -1279,7 +1687,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1279 else 1687 else
1280 { 1688 {
1281 _parent_scene.remActivePrim(this); 1689 _parent_scene.remActivePrim(this);
1282 1690
1283 m_collisionCategories &= ~CollisionCategories.Body; 1691 m_collisionCategories &= ~CollisionCategories.Body;
1284 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); 1692 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
1285 1693
@@ -1289,7 +1697,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1289 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 1697 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1290 } 1698 }
1291 1699
1292 1700
1293 Body = IntPtr.Zero; 1701 Body = IntPtr.Zero;
1294 } 1702 }
1295 } 1703 }
@@ -1345,10 +1753,10 @@ namespace OpenSim.Region.Physics.OdePlugin
1345 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1753 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1346 try 1754 try
1347 { 1755 {
1348 // if (prim_geom == IntPtr.Zero) // setGeom takes care of phys engine recreate and prim_geom pointer 1756 // if (prim_geom == IntPtr.Zero) // setGeom takes care of phys engine recreate and prim_geom pointer
1349 // { 1757 // {
1350 SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null)); 1758 SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
1351 // } 1759 // }
1352 } 1760 }
1353 catch (AccessViolationException) 1761 catch (AccessViolationException)
1354 { 1762 {
@@ -1357,14 +1765,14 @@ namespace OpenSim.Region.Physics.OdePlugin
1357 } 1765 }
1358 1766
1359 1767
1360 // if (IsPhysical && Body == (IntPtr) 0) 1768 // if (IsPhysical && Body == (IntPtr) 0)
1361 // { 1769 // {
1362 // Recreate the body 1770 // Recreate the body
1363 // m_interpenetrationcount = 0; 1771 // m_interpenetrationcount = 0;
1364 // m_collisionscore = 0; 1772 // m_collisionscore = 0;
1365 1773
1366 // enableBody(); 1774 // enableBody();
1367 // } 1775 // }
1368 } 1776 }
1369 1777
1370 public void ProcessTaints(float timestep) //============================================================================= 1778 public void ProcessTaints(float timestep) //=============================================================================
@@ -1373,37 +1781,37 @@ namespace OpenSim.Region.Physics.OdePlugin
1373 { 1781 {
1374 changeadd(timestep); 1782 changeadd(timestep);
1375 } 1783 }
1376 1784
1377 if (prim_geom != IntPtr.Zero) 1785 if (prim_geom != IntPtr.Zero)
1378 { 1786 {
1379 if (!_position.ApproxEquals(m_taintposition, 0f)) 1787 if (!_position.ApproxEquals(m_taintposition, 0f))
1380 { 1788 {
1381 changemove(timestep); 1789 changemove(timestep);
1382 } 1790 }
1383 if (m_taintrot != _orientation) 1791 if (m_taintrot != _orientation)
1384 { 1792 {
1385 if(childPrim && IsPhysical) // For physical child prim... 1793 if (childPrim && IsPhysical) // For physical child prim...
1386 { 1794 {
1387 rotate(timestep); 1795 rotate(timestep);
1388 // KF: ODE will also rotate the parent prim! 1796 // KF: ODE will also rotate the parent prim!
1389 // so rotate the root back to where it was 1797 // so rotate the root back to where it was
1390 OdePrim parent = (OdePrim)_parent; 1798 OdePrim parent = (OdePrim)_parent;
1391 parent.rotate(timestep); 1799 parent.rotate(timestep);
1392 } 1800 }
1393 else 1801 else
1394 { 1802 {
1395 //Just rotate the prim 1803 //Just rotate the prim
1396 rotate(timestep); 1804 rotate(timestep);
1397 } 1805 }
1398 } 1806 }
1399 // 1807 //
1400 1808
1401 if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent)) 1809 if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent))
1402 { 1810 {
1403 changePhysicsStatus(timestep); 1811 changePhysicsStatus(timestep);
1404 }// 1812 }//
1405 1813
1406 if (!_size.ApproxEquals(m_taintsize,0f)) 1814 if (!_size.ApproxEquals(m_taintsize, 0f))
1407 changesize(timestep); 1815 changesize(timestep);
1408 // 1816 //
1409 1817
@@ -1434,10 +1842,14 @@ namespace OpenSim.Region.Physics.OdePlugin
1434 1842
1435 if (m_taintCollidesWater != m_collidesWater) 1843 if (m_taintCollidesWater != m_collidesWater)
1436 changefloatonwater(timestep); 1844 changefloatonwater(timestep);
1437/* obsolete 1845
1438 if (!m_angularLock.ApproxEquals(m_taintAngularLock,0f)) 1846 if (m_taintserial != null)
1439 changeAngularLock(timestep); 1847 DoSerialize(m_taintserial);
1440 */ 1848
1849 /* obsolete
1850 if (!m_angularLock.ApproxEquals(m_taintAngularLock,0f))
1851 changeAngularLock(timestep);
1852 */
1441 } 1853 }
1442 else 1854 else
1443 { 1855 {
@@ -1445,16 +1857,16 @@ namespace OpenSim.Region.Physics.OdePlugin
1445 } 1857 }
1446 } 1858 }
1447 1859
1448/* obsolete 1860 /* obsolete
1449 private void changeAngularLock(float timestep) 1861 private void changeAngularLock(float timestep)
1450 { 1862 {
1451 if (_parent == null) 1863 if (_parent == null)
1452 { 1864 {
1453 m_angularLock = m_taintAngularLock; 1865 m_angularLock = m_taintAngularLock;
1454 m_angularLockSet = true; 1866 m_angularLockSet = true;
1455 } 1867 }
1456 } 1868 }
1457 */ 1869 */
1458 private void changelink(float timestep) 1870 private void changelink(float timestep)
1459 { 1871 {
1460 // If the newly set parent is not null 1872 // If the newly set parent is not null
@@ -1489,7 +1901,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1489 childPrim = false; 1901 childPrim = false;
1490 //_parent = null; 1902 //_parent = null;
1491 } 1903 }
1492 1904
1493 /* 1905 /*
1494 if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0) 1906 if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0)
1495 d.JointGroupDestroy(_linkJointGroup); 1907 d.JointGroupDestroy(_linkJointGroup);
@@ -1498,7 +1910,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1498 m_linkJoint = (IntPtr)0; 1910 m_linkJoint = (IntPtr)0;
1499 */ 1911 */
1500 } 1912 }
1501 1913
1502 _parent = m_taintparent; 1914 _parent = m_taintparent;
1503 m_taintPhysics = m_isphysical; 1915 m_taintPhysics = m_isphysical;
1504 } 1916 }
@@ -1512,8 +1924,8 @@ namespace OpenSim.Region.Physics.OdePlugin
1512 if (Body == IntPtr.Zero) 1924 if (Body == IntPtr.Zero)
1513 { 1925 {
1514 Body = d.BodyCreate(_parent_scene.world); 1926 Body = d.BodyCreate(_parent_scene.world);
1515 // disconnect from world gravity so we can apply buoyancy 1927 // disconnect from world gravity so we can apply buoyancy
1516 d.BodySetGravityMode (Body, false); 1928 d.BodySetGravityMode(Body, false);
1517 1929
1518 setMass(); 1930 setMass();
1519 } 1931 }
@@ -1524,7 +1936,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1524 if (!childrenPrim.Contains(prim)) 1936 if (!childrenPrim.Contains(prim))
1525 { 1937 {
1526 childrenPrim.Add(prim); 1938 childrenPrim.Add(prim);
1527 1939
1528 foreach (OdePrim prm in childrenPrim) 1940 foreach (OdePrim prm in childrenPrim)
1529 { 1941 {
1530 d.Mass m2; 1942 d.Mass m2;
@@ -1546,7 +1958,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1546 } 1958 }
1547 foreach (OdePrim prm in childrenPrim) 1959 foreach (OdePrim prm in childrenPrim)
1548 { 1960 {
1549 1961
1550 prm.m_collisionCategories |= CollisionCategories.Body; 1962 prm.m_collisionCategories |= CollisionCategories.Body;
1551 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); 1963 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1552 1964
@@ -1571,7 +1983,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1571 { 1983 {
1572 d.GeomSetBody(prm.prim_geom, Body); 1984 d.GeomSetBody(prm.prim_geom, Body);
1573 prm.childPrim = true; 1985 prm.childPrim = true;
1574 d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z); 1986 d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X, prm.Position.Y, prm.Position.Z);
1575 //d.GeomSetOffsetPosition(prim.prim_geom, 1987 //d.GeomSetOffsetPosition(prim.prim_geom,
1576 // (Position.X - prm.Position.X) - pMass.c.X, 1988 // (Position.X - prm.Position.X) - pMass.c.X,
1577 // (Position.Y - prm.Position.Y) - pMass.c.Y, 1989 // (Position.Y - prm.Position.Y) - pMass.c.Y,
@@ -1668,7 +2080,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1668 ParentPrim(prm); 2080 ParentPrim(prm);
1669 } 2081 }
1670 } 2082 }
1671 2083
1672 } 2084 }
1673 2085
1674 private void ChildDelink(OdePrim odePrim) 2086 private void ChildDelink(OdePrim odePrim)
@@ -1719,7 +2131,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1719 // in between the disabling and the collision properties setting 2131 // 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 2132 // which would wake the physical body up from a soft disabling and potentially cause it to fall
1721 // through the ground. 2133 // through the ground.
1722 2134
1723 // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select 2135 // 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, 2136 // 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. 2137 // so that causes the selected part to wake up and continue moving.
@@ -1752,12 +2164,12 @@ namespace OpenSim.Region.Physics.OdePlugin
1752 { 2164 {
1753 disableBodySoft(); 2165 disableBodySoft();
1754 } 2166 }
1755 if (Body != IntPtr.Zero) 2167 if (Body != IntPtr.Zero)
1756 { 2168 {
1757 d.BodySetLinearVel(Body, 0f, 0f, 0f); 2169 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1758 d.BodySetForce(Body, 0f, 0f, 0f); 2170 d.BodySetForce(Body, 0f, 0f, 0f);
1759 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); 2171 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
1760 d.BodySetTorque (Body, 0.0f, 0.0f, 0.0f); 2172 d.BodySetTorque(Body, 0.0f, 0.0f, 0.0f);
1761 } 2173 }
1762 2174
1763 } 2175 }
@@ -1780,12 +2192,12 @@ namespace OpenSim.Region.Physics.OdePlugin
1780 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 2192 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1781 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 2193 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1782 } 2194 }
1783 if (Body != IntPtr.Zero) 2195 if (Body != IntPtr.Zero)
1784 { 2196 {
1785 d.BodySetLinearVel(Body, 0f, 0f, 0f); 2197 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1786 d.BodySetForce(Body, 0f, 0f, 0f); 2198 d.BodySetForce(Body, 0f, 0f, 0f);
1787 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); 2199 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
1788 d.BodySetTorque (Body, 0.0f, 0.0f, 0.0f); 2200 d.BodySetTorque(Body, 0.0f, 0.0f, 0.0f);
1789 } 2201 }
1790 2202
1791 if (m_isphysical) 2203 if (m_isphysical)
@@ -1913,7 +2325,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1913 m_meshfailed = true; 2325 m_meshfailed = true;
1914 } 2326 }
1915 // createmesh returns null when it's a shape that isn't a cube. 2327 // createmesh returns null when it's a shape that isn't a cube.
1916 // m_log.Debug(m_localID); 2328 // m_log.Debug(m_localID);
1917 } 2329 }
1918 } 2330 }
1919 2331
@@ -1948,7 +2360,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1948 { 2360 {
1949 if (m_isphysical) 2361 if (m_isphysical)
1950 { 2362 {
1951// if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits! 2363 // if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits!
1952 if (!m_taintremove && !childPrim) 2364 if (!m_taintremove && !childPrim)
1953 { 2365 {
1954 if (Body == IntPtr.Zero) 2366 if (Body == IntPtr.Zero)
@@ -1972,8 +2384,8 @@ namespace OpenSim.Region.Physics.OdePlugin
1972 OdePrim odParent = (OdePrim)_parent; 2384 OdePrim odParent = (OdePrim)_parent;
1973 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body) 2385 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
1974 { 2386 {
1975// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used?? 2387 // KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
1976Console.WriteLine("ODEPrim JointCreateFixed !!!"); 2388 Console.WriteLine("ODEPrim JointCreateFixed !!!");
1977 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); 2389 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
1978 d.JointAttach(m_linkJoint, Body, odParent.Body); 2390 d.JointAttach(m_linkJoint, Body, odParent.Body);
1979 d.JointSetFixed(m_linkJoint); 2391 d.JointSetFixed(m_linkJoint);
@@ -1991,8 +2403,8 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
1991 } 2403 }
1992 } 2404 }
1993 //else 2405 //else
1994 // { 2406 // {
1995 //m_log.Debug("[BUG]: race!"); 2407 //m_log.Debug("[BUG]: race!");
1996 //} 2408 //}
1997 } 2409 }
1998 else 2410 else
@@ -2031,15 +2443,15 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2031 myrot.W = _orientation.W; 2443 myrot.W = _orientation.W;
2032 if (Body != IntPtr.Zero) 2444 if (Body != IntPtr.Zero)
2033 { 2445 {
2034 // KF: If this is a root prim do BodySet 2446 // KF: If this is a root prim do BodySet
2035 d.BodySetQuaternion(Body, ref myrot); 2447 d.BodySetQuaternion(Body, ref myrot);
2036 }
2037 else
2038 {
2039 // daughter prim, do Geom set
2040 d.GeomSetQuaternion(prim_geom, ref myrot);
2041 } 2448 }
2042 2449 else
2450 {
2451 // daughter prim, do Geom set
2452 d.GeomSetQuaternion(prim_geom, ref myrot);
2453 }
2454
2043 resetCollisionAccounting(); 2455 resetCollisionAccounting();
2044 m_taintrot = _orientation; 2456 m_taintrot = _orientation;
2045 } 2457 }
@@ -2111,7 +2523,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2111 2523
2112 public void changesize(float timestamp) 2524 public void changesize(float timestamp)
2113 { 2525 {
2114 2526
2115 string oldname = _parent_scene.geom_name_map[prim_geom]; 2527 string oldname = _parent_scene.geom_name_map[prim_geom];
2116 2528
2117 if (_size.X <= 0) _size.X = 0.01f; 2529 if (_size.X <= 0) _size.X = 0.01f;
@@ -2170,7 +2582,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2170 //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); 2582 //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
2171 CreateGeom(m_targetSpace, mesh); 2583 CreateGeom(m_targetSpace, mesh);
2172 2584
2173 2585
2174 } 2586 }
2175 else 2587 else
2176 { 2588 {
@@ -2210,7 +2622,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2210 m_taintsize = _size; 2622 m_taintsize = _size;
2211 } 2623 }
2212 2624
2213 2625
2214 2626
2215 public void changefloatonwater(float timestep) 2627 public void changefloatonwater(float timestep)
2216 { 2628 {
@@ -2398,7 +2810,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2398 } 2810 }
2399 d.BodyEnable(Body); 2811 d.BodyEnable(Body);
2400 d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z); 2812 d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z);
2401 2813
2402 } 2814 }
2403 m_angularforcelist.Clear(); 2815 m_angularforcelist.Clear();
2404 } 2816 }
@@ -2420,7 +2832,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2420 if (Body != IntPtr.Zero) 2832 if (Body != IntPtr.Zero)
2421 d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z); 2833 d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
2422 } 2834 }
2423 2835
2424 //resetCollisionAccounting(); 2836 //resetCollisionAccounting();
2425 } 2837 }
2426 m_taintVelocity = Vector3.Zero; 2838 m_taintVelocity = Vector3.Zero;
@@ -2428,9 +2840,9 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2428 2840
2429 public void UpdatePositionAndVelocity() 2841 public void UpdatePositionAndVelocity()
2430 { 2842 {
2431 return; // moved to the Move () method 2843 return; // moved to the Move () method
2432 } 2844 }
2433 2845
2434 public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj) 2846 public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
2435 { 2847 {
2436 obj.I.M00 = pMat[0, 0]; 2848 obj.I.M00 = pMat[0, 0];
@@ -2455,7 +2867,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2455 { 2867 {
2456 _parent_scene.remCollisionEventReporting(this); 2868 _parent_scene.remCollisionEventReporting(this);
2457 m_eventsubscription = 0; 2869 m_eventsubscription = 0;
2458 } 2870 }
2459 2871
2460 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) 2872 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
2461 { 2873 {
@@ -2499,9 +2911,9 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2499 public static Matrix4 Adjoint(Matrix4 pMat) 2911 public static Matrix4 Adjoint(Matrix4 pMat)
2500 { 2912 {
2501 Matrix4 adjointMatrix = new Matrix4(); 2913 Matrix4 adjointMatrix = new Matrix4();
2502 for (int i=0; i<4; i++) 2914 for (int i = 0; i < 4; i++)
2503 { 2915 {
2504 for (int j=0; j<4; j++) 2916 for (int j = 0; j < 4; j++)
2505 { 2917 {
2506 Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j))))); 2918 Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j)))));
2507 } 2919 }
@@ -2524,7 +2936,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2524 { 2936 {
2525 if (j == iCol) 2937 if (j == iCol)
2526 continue; 2938 continue;
2527 Matrix4SetValue(ref minor, m,n, matrix[i, j]); 2939 Matrix4SetValue(ref minor, m, n, matrix[i, j]);
2528 n++; 2940 n++;
2529 } 2941 }
2530 m++; 2942 m++;
@@ -2622,18 +3034,18 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2622 private static float determinant3x3(Matrix4 pMat) 3034 private static float determinant3x3(Matrix4 pMat)
2623 { 3035 {
2624 float det = 0; 3036 float det = 0;
2625 float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2]; 3037 float diag1 = pMat[0, 0] * pMat[1, 1] * pMat[2, 2];
2626 float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0]; 3038 float diag2 = pMat[0, 1] * pMat[2, 1] * pMat[2, 0];
2627 float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1]; 3039 float diag3 = pMat[0, 2] * pMat[1, 0] * pMat[2, 1];
2628 float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2]; 3040 float diag4 = pMat[2, 0] * pMat[1, 1] * pMat[0, 2];
2629 float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0]; 3041 float diag5 = pMat[2, 1] * pMat[1, 2] * pMat[0, 0];
2630 float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1]; 3042 float diag6 = pMat[2, 2] * pMat[1, 0] * pMat[0, 1];
2631 3043
2632 det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6); 3044 det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6);
2633 return det; 3045 return det;
2634 3046
2635 } 3047 }
2636 3048
2637 private static void DMassCopy(ref d.Mass src, ref d.Mass dst) 3049 private static void DMassCopy(ref d.Mass src, ref d.Mass dst)
2638 { 3050 {
2639 dst.c.W = src.c.W; 3051 dst.c.W = src.c.W;
@@ -2690,15 +3102,15 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2690 // m_bankingTimescale = pValue; 3102 // m_bankingTimescale = pValue;
2691 break; 3103 break;
2692 case Vehicle.BUOYANCY: 3104 case Vehicle.BUOYANCY:
2693 if (pValue < -1f) pValue = -1f; 3105 if (pValue < -1f) pValue = -1f;
2694 if (pValue > 1f) pValue = 1f; 3106 if (pValue > 1f) pValue = 1f;
2695 m_VehicleBuoyancy = pValue; 3107 m_VehicleBuoyancy = pValue;
2696 break; 3108 break;
2697// case Vehicle.HOVER_EFFICIENCY: 3109 // case Vehicle.HOVER_EFFICIENCY:
2698// if (pValue < 0f) pValue = 0f; 3110 // if (pValue < 0f) pValue = 0f;
2699// if (pValue > 1f) pValue = 1f; 3111 // if (pValue > 1f) pValue = 1f;
2700// m_VhoverEfficiency = pValue; 3112 // m_VhoverEfficiency = pValue;
2701// break; 3113 // break;
2702 case Vehicle.HOVER_HEIGHT: 3114 case Vehicle.HOVER_HEIGHT:
2703 m_VhoverHeight = pValue; 3115 m_VhoverHeight = pValue;
2704 break; 3116 break;
@@ -2731,12 +3143,12 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2731 if (pValue < 0.1f) pValue = 0.1f; 3143 if (pValue < 0.1f) pValue = 0.1f;
2732 m_verticalAttractionTimescale = pValue; 3144 m_verticalAttractionTimescale = pValue;
2733 break; 3145 break;
2734 3146
2735 // These are vector properties but the engine lets you use a single float value to 3147 // 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 3148 // set all of the components to the same value
2737 case Vehicle.ANGULAR_FRICTION_TIMESCALE: 3149 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
2738 if (pValue > 30f) pValue = 30f; 3150 if (pValue > 30f) pValue = 30f;
2739 if (pValue < 0.1f) pValue = 0.1f; 3151 if (pValue < 0.1f) pValue = 0.1f;
2740 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); 3152 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
2741 break; 3153 break;
2742 case Vehicle.ANGULAR_MOTOR_DIRECTION: 3154 case Vehicle.ANGULAR_MOTOR_DIRECTION:
@@ -2744,7 +3156,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2744 UpdateAngDecay(); 3156 UpdateAngDecay();
2745 break; 3157 break;
2746 case Vehicle.LINEAR_FRICTION_TIMESCALE: 3158 case Vehicle.LINEAR_FRICTION_TIMESCALE:
2747 if (pValue < 0.1f) pValue = 0.1f; 3159 if (pValue < 0.1f) pValue = 0.1f;
2748 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); 3160 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
2749 break; 3161 break;
2750 case Vehicle.LINEAR_MOTOR_DIRECTION: 3162 case Vehicle.LINEAR_MOTOR_DIRECTION:
@@ -2756,7 +3168,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2756 break; 3168 break;
2757 3169
2758 } 3170 }
2759 3171
2760 }//end ProcessFloatVehicleParam 3172 }//end ProcessFloatVehicleParam
2761 3173
2762 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) 3174 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
@@ -2764,29 +3176,29 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2764 switch (pParam) 3176 switch (pParam)
2765 { 3177 {
2766 case Vehicle.ANGULAR_FRICTION_TIMESCALE: 3178 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
2767 if (pValue.X > 30f) pValue.X = 30f; 3179 if (pValue.X > 30f) pValue.X = 30f;
2768 if (pValue.X < 0.1f) pValue.X = 0.1f; 3180 if (pValue.X < 0.1f) pValue.X = 0.1f;
2769 if (pValue.Y > 30f) pValue.Y = 30f; 3181 if (pValue.Y > 30f) pValue.Y = 30f;
2770 if (pValue.Y < 0.1f) pValue.Y = 0.1f; 3182 if (pValue.Y < 0.1f) pValue.Y = 0.1f;
2771 if (pValue.Z > 30f) pValue.Z = 30f; 3183 if (pValue.Z > 30f) pValue.Z = 30f;
2772 if (pValue.Z < 0.1f) pValue.Z = 0.1f; 3184 if (pValue.Z < 0.1f) pValue.Z = 0.1f;
2773 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); 3185 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
2774 break; 3186 break;
2775 case Vehicle.ANGULAR_MOTOR_DIRECTION: 3187 case Vehicle.ANGULAR_MOTOR_DIRECTION:
2776 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); 3188 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
2777 // Limit requested angular speed to 2 rps= 4 pi rads/sec 3189 // Limit requested angular speed to 2 rps= 4 pi rads/sec
2778 if(m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f; 3190 if (m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f;
2779 if(m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f; 3191 if (m_angularMotorDirection.X < -12.56f) m_angularMotorDirection.X = -12.56f;
2780 if(m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f; 3192 if (m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f;
2781 if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f; 3193 if (m_angularMotorDirection.Y < -12.56f) m_angularMotorDirection.Y = -12.56f;
2782 if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f; 3194 if (m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f;
2783 if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f; 3195 if (m_angularMotorDirection.Z < -12.56f) m_angularMotorDirection.Z = -12.56f;
2784 UpdateAngDecay(); 3196 UpdateAngDecay();
2785 break; 3197 break;
2786 case Vehicle.LINEAR_FRICTION_TIMESCALE: 3198 case Vehicle.LINEAR_FRICTION_TIMESCALE:
2787 if (pValue.X < 0.1f) pValue.X = 0.1f; 3199 if (pValue.X < 0.1f) pValue.X = 0.1f;
2788 if (pValue.Y < 0.1f) pValue.Y = 0.1f; 3200 if (pValue.Y < 0.1f) pValue.Y = 0.1f;
2789 if (pValue.Z < 0.1f) pValue.Z = 0.1f; 3201 if (pValue.Z < 0.1f) pValue.Z = 0.1f;
2790 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); 3202 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
2791 break; 3203 break;
2792 case Vehicle.LINEAR_MOTOR_DIRECTION: 3204 case Vehicle.LINEAR_MOTOR_DIRECTION:
@@ -2797,7 +3209,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2797 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); 3209 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
2798 break; 3210 break;
2799 } 3211 }
2800 3212
2801 }//end ProcessVectorVehicleParam 3213 }//end ProcessVectorVehicleParam
2802 3214
2803 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) 3215 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
@@ -2808,31 +3220,31 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2808 // m_referenceFrame = pValue; 3220 // m_referenceFrame = pValue;
2809 break; 3221 break;
2810 } 3222 }
2811 3223
2812 }//end ProcessRotationVehicleParam 3224 }//end ProcessRotationVehicleParam
2813 3225
2814 internal void ProcessVehicleFlags(int pParam, bool remove) 3226 internal void ProcessVehicleFlags(int pParam, bool remove)
2815 { 3227 {
2816 if (remove) 3228 if (remove)
2817 { 3229 {
2818 m_flags &= ~((VehicleFlag)pParam); 3230 m_flags &= ~((VehicleFlag)pParam);
2819 } 3231 }
2820 else 3232 else
2821 { 3233 {
2822 m_flags |= (VehicleFlag)pParam; 3234 m_flags |= (VehicleFlag)pParam;
2823 } 3235 }
2824 } 3236 }
2825 3237
2826 internal void ProcessTypeChange(Vehicle pType) 3238 internal void ProcessTypeChange(Vehicle pType)
2827 { 3239 {
2828 // Set Defaults For Type 3240 // Set Defaults For Type
2829 m_type = pType; 3241 m_type = pType;
2830 switch (pType) 3242 switch (pType)
2831 { 3243 {
2832 case Vehicle.TYPE_SLED: 3244 case Vehicle.TYPE_SLED:
2833 m_linearFrictionTimescale = new Vector3(30, 1, 1000); 3245 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
2834 m_angularFrictionTimescale = new Vector3(30, 30, 30); 3246 m_angularFrictionTimescale = new Vector3(30, 30, 30);
2835// m_lLinMotorVel = Vector3.Zero; 3247 // m_lLinMotorVel = Vector3.Zero;
2836 m_linearMotorTimescale = 1000; 3248 m_linearMotorTimescale = 1000;
2837 m_linearMotorDecayTimescale = 120; 3249 m_linearMotorDecayTimescale = 120;
2838 m_angularMotorDirection = Vector3.Zero; 3250 m_angularMotorDirection = Vector3.Zero;
@@ -2840,7 +3252,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2840 m_angularMotorTimescale = 1000; 3252 m_angularMotorTimescale = 1000;
2841 m_angularMotorDecayTimescale = 120; 3253 m_angularMotorDecayTimescale = 120;
2842 m_VhoverHeight = 0; 3254 m_VhoverHeight = 0;
2843// m_VhoverEfficiency = 1; 3255 // m_VhoverEfficiency = 1;
2844 m_VhoverTimescale = 10; 3256 m_VhoverTimescale = 10;
2845 m_VehicleBuoyancy = 0; 3257 m_VehicleBuoyancy = 0;
2846 // m_linearDeflectionEfficiency = 1; 3258 // m_linearDeflectionEfficiency = 1;
@@ -2859,7 +3271,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2859 case Vehicle.TYPE_CAR: 3271 case Vehicle.TYPE_CAR:
2860 m_linearFrictionTimescale = new Vector3(100, 2, 1000); 3272 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
2861 m_angularFrictionTimescale = new Vector3(30, 30, 30); // was 1000, but sl max frict time is 30. 3273 m_angularFrictionTimescale = new Vector3(30, 30, 30); // was 1000, but sl max frict time is 30.
2862// m_lLinMotorVel = Vector3.Zero; 3274 // m_lLinMotorVel = Vector3.Zero;
2863 m_linearMotorTimescale = 1; 3275 m_linearMotorTimescale = 1;
2864 m_linearMotorDecayTimescale = 60; 3276 m_linearMotorDecayTimescale = 60;
2865 m_angularMotorDirection = Vector3.Zero; 3277 m_angularMotorDirection = Vector3.Zero;
@@ -2867,7 +3279,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2867 m_angularMotorTimescale = 1; 3279 m_angularMotorTimescale = 1;
2868 m_angularMotorDecayTimescale = 0.8f; 3280 m_angularMotorDecayTimescale = 0.8f;
2869 m_VhoverHeight = 0; 3281 m_VhoverHeight = 0;
2870// m_VhoverEfficiency = 0; 3282 // m_VhoverEfficiency = 0;
2871 m_VhoverTimescale = 1000; 3283 m_VhoverTimescale = 1000;
2872 m_VehicleBuoyancy = 0; 3284 m_VehicleBuoyancy = 0;
2873 // // m_linearDeflectionEfficiency = 1; 3285 // // m_linearDeflectionEfficiency = 1;
@@ -2886,8 +3298,8 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2886 break; 3298 break;
2887 case Vehicle.TYPE_BOAT: 3299 case Vehicle.TYPE_BOAT:
2888 m_linearFrictionTimescale = new Vector3(10, 3, 2); 3300 m_linearFrictionTimescale = new Vector3(10, 3, 2);
2889 m_angularFrictionTimescale = new Vector3(10,10,10); 3301 m_angularFrictionTimescale = new Vector3(10, 10, 10);
2890// m_lLinMotorVel = Vector3.Zero; 3302 // m_lLinMotorVel = Vector3.Zero;
2891 m_linearMotorTimescale = 5; 3303 m_linearMotorTimescale = 5;
2892 m_linearMotorDecayTimescale = 60; 3304 m_linearMotorDecayTimescale = 60;
2893 m_angularMotorDirection = Vector3.Zero; 3305 m_angularMotorDirection = Vector3.Zero;
@@ -2895,7 +3307,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2895 m_angularMotorTimescale = 4; 3307 m_angularMotorTimescale = 4;
2896 m_angularMotorDecayTimescale = 4; 3308 m_angularMotorDecayTimescale = 4;
2897 m_VhoverHeight = 0; 3309 m_VhoverHeight = 0;
2898// m_VhoverEfficiency = 0.5f; 3310 // m_VhoverEfficiency = 0.5f;
2899 m_VhoverTimescale = 2; 3311 m_VhoverTimescale = 2;
2900 m_VehicleBuoyancy = 1; 3312 m_VehicleBuoyancy = 1;
2901 // m_linearDeflectionEfficiency = 0.5f; 3313 // m_linearDeflectionEfficiency = 0.5f;
@@ -2908,15 +3320,15 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2908 // m_bankingMix = 0.8f; 3320 // m_bankingMix = 0.8f;
2909 // m_bankingTimescale = 1; 3321 // m_bankingTimescale = 1;
2910 // m_referenceFrame = Quaternion.Identity; 3322 // m_referenceFrame = Quaternion.Identity;
2911 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY | 3323 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY |
2912 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); 3324 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
2913 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | 3325 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY |
2914 VehicleFlag.LIMIT_MOTOR_UP); 3326 VehicleFlag.LIMIT_MOTOR_UP);
2915 break; 3327 break;
2916 case Vehicle.TYPE_AIRPLANE: 3328 case Vehicle.TYPE_AIRPLANE:
2917 m_linearFrictionTimescale = new Vector3(200, 10, 5); 3329 m_linearFrictionTimescale = new Vector3(200, 10, 5);
2918 m_angularFrictionTimescale = new Vector3(20, 20, 20); 3330 m_angularFrictionTimescale = new Vector3(20, 20, 20);
2919// m_lLinMotorVel = Vector3.Zero; 3331 // m_lLinMotorVel = Vector3.Zero;
2920 m_linearMotorTimescale = 2; 3332 m_linearMotorTimescale = 2;
2921 m_linearMotorDecayTimescale = 60; 3333 m_linearMotorDecayTimescale = 60;
2922 m_angularMotorDirection = Vector3.Zero; 3334 m_angularMotorDirection = Vector3.Zero;
@@ -2924,7 +3336,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2924 m_angularMotorTimescale = 4; 3336 m_angularMotorTimescale = 4;
2925 m_angularMotorDecayTimescale = 4; 3337 m_angularMotorDecayTimescale = 4;
2926 m_VhoverHeight = 0; 3338 m_VhoverHeight = 0;
2927// m_VhoverEfficiency = 0.5f; 3339 // m_VhoverEfficiency = 0.5f;
2928 m_VhoverTimescale = 1000; 3340 m_VhoverTimescale = 1000;
2929 m_VehicleBuoyancy = 0; 3341 m_VehicleBuoyancy = 0;
2930 // m_linearDeflectionEfficiency = 0.5f; 3342 // m_linearDeflectionEfficiency = 0.5f;
@@ -2951,7 +3363,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2951 m_angularMotorTimescale = 6; 3363 m_angularMotorTimescale = 6;
2952 m_angularMotorDecayTimescale = 10; 3364 m_angularMotorDecayTimescale = 10;
2953 m_VhoverHeight = 5; 3365 m_VhoverHeight = 5;
2954// m_VhoverEfficiency = 0.8f; 3366 // m_VhoverEfficiency = 0.8f;
2955 m_VhoverTimescale = 10; 3367 m_VhoverTimescale = 10;
2956 m_VehicleBuoyancy = 1; 3368 m_VehicleBuoyancy = 1;
2957 // m_linearDeflectionEfficiency = 0; 3369 // m_linearDeflectionEfficiency = 0;
@@ -2981,63 +3393,48 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2981 } 3393 }
2982 3394
2983 3395
2984 internal void Halt() 3396 internal void Halt()
2985 { // Kill all motions, when non-physical 3397 { // Kill all motions, when non-physical
2986 // m_linearMotorDirection = Vector3.Zero; 3398 // m_linearMotorDirection = Vector3.Zero;
2987 m_lLinMotorDVel = Vector3.Zero; 3399 m_lLinMotorDVel = Vector3.Zero;
2988 m_lLinObjectVel = Vector3.Zero; 3400 m_lLinObjectVel = Vector3.Zero;
2989 m_wLinObjectVel = Vector3.Zero; 3401 m_wLinObjectVel = Vector3.Zero;
2990 m_angularMotorDirection = Vector3.Zero; 3402 m_angularMotorDirection = Vector3.Zero;
2991 m_lastAngularVelocity = Vector3.Zero; 3403 m_lastAngularVelocity = Vector3.Zero;
2992 m_angularMotorDVel = Vector3.Zero; 3404 m_angularMotorDVel = Vector3.Zero;
2993 _acceleration = Vector3.Zero; 3405 _acceleration = Vector3.Zero;
2994 } 3406 }
2995 3407
2996 private void UpdateLinDecay() 3408 private void UpdateLinDecay()
2997 { 3409 {
2998// if (Math.Abs(m_linearMotorDirection.X) > Math.Abs(m_lLinMotorDVel.X)) m_lLinMotorDVel.X = m_linearMotorDirection.X; 3410 m_lLinMotorDVel.X = m_linearMotorDirection.X;
2999// if (Math.Abs(m_linearMotorDirection.Y) > Math.Abs(m_lLinMotorDVel.Y)) m_lLinMotorDVel.Y = m_linearMotorDirection.Y; 3411 m_lLinMotorDVel.Y = m_linearMotorDirection.Y;
3000// if (Math.Abs(m_linearMotorDirection.Z) > Math.Abs(m_lLinMotorDVel.Z)) m_lLinMotorDVel.Z = m_linearMotorDirection.Z; 3412 m_lLinMotorDVel.Z = m_linearMotorDirection.Z;
3001 m_lLinMotorDVel.X = m_linearMotorDirection.X; 3413 } // else let the motor decay on its own
3002 m_lLinMotorDVel.Y = m_linearMotorDirection.Y; 3414
3003 m_lLinMotorDVel.Z = m_linearMotorDirection.Z; 3415 private void UpdateAngDecay()
3004 } // else let the motor decay on its own 3416 {
3005 3417 m_angularMotorDVel.X = m_angularMotorDirection.X;
3006 private void UpdateAngDecay() 3418 m_angularMotorDVel.Y = m_angularMotorDirection.Y;
3007 { 3419 m_angularMotorDVel.Z = m_angularMotorDirection.Z;
3008// if (Math.Abs(m_angularMotorDirection.X) > Math.Abs(m_angularMotorDVel.X)) m_angularMotorDVel.X = m_angularMotorDirection.X; 3420 } // 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; 3421
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) 3422 public void Move(float timestep)
3017 { 3423 {
3018 float fx = 0; 3424 float fx = 0;
3019 float fy = 0; 3425 float fy = 0;
3020 float fz = 0; 3426 float fz = 0;
3021 Vector3 linvel; // velocity applied, including any reversal 3427 Vector3 linvel; // velocity applied, including any reversal
3022 int outside = 0; 3428
3023
3024 // If geomCrossingFailuresBeforeOutofbounds is set to 0 in OpenSim.ini then phys objects bounce off region borders. 3429 // 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. 3430 // This is a temp patch until proper region crossing is developed.
3026 3431
3027 int failureLimit = _parent_scene.geomCrossingFailuresBeforeOutofbounds; 3432
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. 3433 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim && !m_outofBounds) // Only move root prims.
3037 { 3434 {
3038 // Old public void UpdatePositionAndVelocity(), more accuratley calculated here 3435 // Old public void UpdatePositionAndVelocity(), more accuratley calculated here
3039 bool lastZeroFlag = _zeroFlag; // was it stopped 3436 bool lastZeroFlag = _zeroFlag; // was it stopped
3040 3437
3041 d.Vector3 vec = d.BodyGetPosition(Body); 3438 d.Vector3 vec = d.BodyGetPosition(Body);
3042 Vector3 l_position = Vector3.Zero; 3439 Vector3 l_position = Vector3.Zero;
3043 l_position.X = vec.X; 3440 l_position.X = vec.X;
@@ -3045,95 +3442,26 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
3045 l_position.Z = vec.Z; 3442 l_position.Z = vec.Z;
3046 m_lastposition = _position; 3443 m_lastposition = _position;
3047 _position = l_position; 3444 _position = l_position;
3048 3445
3049 d.Quaternion ori = d.BodyGetQuaternion(Body); 3446 d.Quaternion ori = d.BodyGetQuaternion(Body);
3050 // Quaternion l_orientation = Quaternion.Identity; 3447 // Quaternion l_orientation = Quaternion.Identity;
3051 _orientation.X = ori.X; 3448 _orientation.X = ori.X;
3052 _orientation.Y = ori.Y; 3449 _orientation.Y = ori.Y;
3053 _orientation.Z = ori.Z; 3450 _orientation.Z = ori.Z;
3054 _orientation.W = ori.W; 3451 _orientation.W = ori.W;
3055 m_lastorientation = _orientation; 3452 m_lastorientation = _orientation;
3056 3453
3057 d.Vector3 vel = d.BodyGetLinearVel(Body); 3454 d.Vector3 vel = d.BodyGetLinearVel(Body);
3058 m_lastVelocity = _velocity; 3455 m_lastVelocity = _velocity;
3059 _velocity.X = vel.X; 3456 _velocity.X = vel.X;
3060 _velocity.Y = vel.Y; 3457 _velocity.Y = vel.Y;
3061 _velocity.Z = vel.Z; 3458 _velocity.Z = vel.Z;
3062 _acceleration = ((_velocity - m_lastVelocity) / timestep); 3459 _acceleration = ((_velocity - m_lastVelocity) / timestep);
3063 3460
3064 d.Vector3 torque = d.BodyGetTorque(Body); 3461 d.Vector3 torque = d.BodyGetTorque(Body);
3065 _torque = new Vector3(torque.X, torque.Y, torque.Z); 3462 _torque = new Vector3(torque.X, torque.Y, torque.Z);
3066 3463
3067 3464
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 3465 if (_position.X < 0f || _position.X > _parent_scene.WorldExtents.X
3138 || _position.Y < 0f || _position.Y > _parent_scene.WorldExtents.Y 3466 || _position.Y < 0f || _position.Y > _parent_scene.WorldExtents.Y
3139 ) 3467 )
@@ -3146,41 +3474,11 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
3146 _position.Z = Util.Clip(l_position.Z, -100f, 50000f); 3474 _position.Z = Util.Clip(l_position.Z, -100f, 50000f);
3147 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); 3475 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
3148 d.BodySetLinearVel(Body, 0, 0, 0); 3476 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; 3477 m_outofBounds = true;
3157 base.RequestPhysicsterseUpdate(); 3478 base.RequestPhysicsterseUpdate();
3158 return; 3479 return;
3159 } 3480 }
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
3170 m_lastposition = _position;
3171
3172 _velocity = Vector3.Zero;
3173 m_lastVelocity = _velocity;
3174 3481
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(); 3482 base.RequestPhysicsterseUpdate();
3185 3483
3186 if (l_position.Z < 0) 3484 if (l_position.Z < 0)
@@ -3193,8 +3491,8 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
3193 3491
3194 //IsPhysical = false; 3492 //IsPhysical = false;
3195 if (_parent == null) base.RaiseOutOfBounds(_position); 3493 if (_parent == null) base.RaiseOutOfBounds(_position);
3196 3494
3197 3495
3198 _acceleration.X = 0; // This stuff may stop client display but it has no 3496 _acceleration.X = 0; // This stuff may stop client display but it has no
3199 _acceleration.Y = 0; // effect on the object in phys engine! 3497 _acceleration.Y = 0; // effect on the object in phys engine!
3200 _acceleration.Z = 0; 3498 _acceleration.Z = 0;
@@ -3215,13 +3513,13 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
3215 //outofBounds = true; 3513 //outofBounds = true;
3216 } // end neg Z check 3514 } // end neg Z check
3217 3515
3218 // Is it moving? 3516 // Is it moving?
3219 /* if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) 3517 /* if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
3220 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) 3518 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
3221 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) */ 3519 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) */
3222 if ( (Vector3.Mag(_velocity) < 0.01) && // moving very slowly 3520 if ((Vector3.Mag(_velocity) < 0.01) && // moving very slowly
3223 (Vector3.Mag(_velocity) < Vector3.Mag(m_lastVelocity)) && // decelerating 3521 (Vector3.Mag(_velocity) < Vector3.Mag(m_lastVelocity)) && // decelerating
3224 (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001) ) // spinning very slowly 3522 (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001)) // spinning very slowly
3225 { 3523 {
3226 _zeroFlag = true; 3524 _zeroFlag = true;
3227 m_throttleUpdates = false; 3525 m_throttleUpdates = false;
@@ -3238,18 +3536,18 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
3238 { // Its stopped 3536 { // Its stopped
3239 _velocity.X = 0.0f; 3537 _velocity.X = 0.0f;
3240 _velocity.Y = 0.0f; 3538 _velocity.Y = 0.0f;
3241 // _velocity.Z = 0.0f; 3539 // _velocity.Z = 0.0f;
3242 3540
3243 _acceleration.X = 0; 3541 _acceleration.X = 0;
3244 _acceleration.Y = 0; 3542 _acceleration.Y = 0;
3245 // _acceleration.Z = 0; 3543 // _acceleration.Z = 0;
3246 3544
3247 m_rotationalVelocity.X = 0; 3545 m_rotationalVelocity.X = 0;
3248 m_rotationalVelocity.Y = 0; 3546 m_rotationalVelocity.Y = 0;
3249 m_rotationalVelocity.Z = 0; 3547 m_rotationalVelocity.Z = 0;
3250 // Stop it in the phys engine 3548 // Stop it in the phys engine
3251 d.BodySetLinearVel(Body, 0.0f, 0.0f, _velocity.Z); 3549 d.BodySetLinearVel(Body, 0.0f, 0.0f, _velocity.Z);
3252 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); 3550 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
3253 d.BodySetForce(Body, 0f, 0f, 0f); 3551 d.BodySetForce(Body, 0f, 0f, 0f);
3254 3552
3255 if (!m_lastUpdateSent) 3553 if (!m_lastUpdateSent)
@@ -3287,71 +3585,71 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
3287 } 3585 }
3288 } 3586 }
3289 m_lastposition = l_position; 3587 m_lastposition = l_position;
3290 3588
3291 /// End UpdatePositionAndVelocity insert 3589 /// End UpdatePositionAndVelocity insert
3292 3590
3293 3591
3294 // Rotation lock ===================================== 3592 // Rotation lock =====================================
3295 if(m_rotateEnableUpdate) 3593 if (m_rotateEnableUpdate)
3296 { 3594 {
3297 // Snapshot current angles, set up Amotor(s) 3595 // Snapshot current angles, set up Amotor(s)
3298 m_rotateEnableUpdate = false; 3596 m_rotateEnableUpdate = false;
3299 m_rotateEnable = m_rotateEnableRequest; 3597 m_rotateEnable = m_rotateEnableRequest;
3300//Console.WriteLine("RotEnable {0} = {1}",m_primName, m_rotateEnable); 3598 //Console.WriteLine("RotEnable {0} = {1}",m_primName, m_rotateEnable);
3301 3599
3302 if (Amotor != IntPtr.Zero) 3600 if (Amotor != IntPtr.Zero)
3303 { 3601 {
3304 d.JointDestroy(Amotor); 3602 d.JointDestroy(Amotor);
3305 Amotor = IntPtr.Zero; 3603 Amotor = IntPtr.Zero;
3306//Console.WriteLine("Old Amotor Destroyed"); 3604 //Console.WriteLine("Old Amotor Destroyed");
3307 } 3605 }
3308 3606
3309 if (!m_rotateEnable.ApproxEquals(Vector3.One, 0.003f)) 3607 if (!m_rotateEnable.ApproxEquals(Vector3.One, 0.003f))
3310 { // not all are enabled 3608 { // not all are enabled
3311 d.Quaternion r = d.BodyGetQuaternion(Body); 3609 d.Quaternion r = d.BodyGetQuaternion(Body);
3312 Quaternion locrot = new Quaternion(r.X, r.Y, r.Z, r.W); 3610 Quaternion locrot = new Quaternion(r.X, r.Y, r.Z, r.W);
3313 // extract the axes vectors 3611 // extract the axes vectors
3314 Vector3 vX = new Vector3(1f,0f,0f); 3612 Vector3 vX = new Vector3(1f, 0f, 0f);
3315 Vector3 vY = new Vector3(0f,1f,0f); 3613 Vector3 vY = new Vector3(0f, 1f, 0f);
3316 Vector3 vZ = new Vector3(0f,0f,1f); 3614 Vector3 vZ = new Vector3(0f, 0f, 1f);
3317 vX = vX * locrot; 3615 vX = vX * locrot;
3318 vY = vY * locrot; 3616 vY = vY * locrot;
3319 vZ = vZ * locrot; 3617 vZ = vZ * locrot;
3320 // snapshot the current angle vectors 3618 // snapshot the current angle vectors
3321 m_lockX = vX; 3619 m_lockX = vX;
3322 m_lockY = vY; 3620 m_lockY = vY;
3323 m_lockZ = vZ; 3621 m_lockZ = vZ;
3324 // m_lockRot = locrot; 3622 // m_lockRot = locrot;
3325 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); 3623 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
3326 d.JointAttach(Amotor, Body, IntPtr.Zero); 3624 d.JointAttach(Amotor, Body, IntPtr.Zero);
3327 d.JointSetAMotorMode(Amotor, 0); // User mode?? 3625 d.JointSetAMotorMode(Amotor, 0); // User mode??
3328//Console.WriteLine("New Amotor Created for {0}", m_primName); 3626 //Console.WriteLine("New Amotor Created for {0}", m_primName);
3329 3627
3330 float axisnum = 3; // how many to lock 3628 float axisnum = 3; // how many to lock
3331 axisnum = (axisnum - (m_rotateEnable.X + m_rotateEnable.Y + m_rotateEnable.Z)); 3629 axisnum = (axisnum - (m_rotateEnable.X + m_rotateEnable.Y + m_rotateEnable.Z));
3332 d.JointSetAMotorNumAxes(Amotor,(int)axisnum); 3630 d.JointSetAMotorNumAxes(Amotor, (int)axisnum);
3333//Console.WriteLine("AxisNum={0}",(int)axisnum); 3631 //Console.WriteLine("AxisNum={0}",(int)axisnum);
3334 3632
3335 int i = 0; 3633 int i = 0;
3336 3634
3337 if (m_rotateEnable.X == 0) 3635 if (m_rotateEnable.X == 0)
3338 { 3636 {
3339 d.JointSetAMotorAxis(Amotor, i, 0, m_lockX.X, m_lockX.Y, m_lockX.Z); 3637 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); 3638 //Console.WriteLine("AxisX {0} set to {1}", i, m_lockX);
3341 i++; 3639 i++;
3342 } 3640 }
3343 3641
3344 if (m_rotateEnable.Y == 0) 3642 if (m_rotateEnable.Y == 0)
3345 { 3643 {
3346 d.JointSetAMotorAxis(Amotor, i, 0, m_lockY.X, m_lockY.Y, m_lockY.Z); 3644 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); 3645 //Console.WriteLine("AxisY {0} set to {1}", i, m_lockY);
3348 i++; 3646 i++;
3349 } 3647 }
3350 3648
3351 if (m_rotateEnable.Z == 0) 3649 if (m_rotateEnable.Z == 0)
3352 { 3650 {
3353 d.JointSetAMotorAxis(Amotor, i, 0, m_lockZ.X, m_lockZ.Y, m_lockZ.Z); 3651 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); 3652 //Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ);
3355 i++; 3653 i++;
3356 } 3654 }
3357 3655
@@ -3362,519 +3660,519 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
3362 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f); 3660 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
3363 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); 3661 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
3364 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f); 3662 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
3365 d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 0f); 3663 d.JointSetAMotorParam(Amotor, (int)dParam.Vel, 0f);
3366 d.JointSetAMotorParam(Amotor, (int) dParam.Vel3, 0f); 3664 d.JointSetAMotorParam(Amotor, (int)dParam.Vel3, 0f);
3367 d.JointSetAMotorParam(Amotor, (int) dParam.Vel2, 0f); 3665 d.JointSetAMotorParam(Amotor, (int)dParam.Vel2, 0f);
3368 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); 3666 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f);
3369 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f); 3667 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f);
3370 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f); 3668 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f);
3371 } // else none are locked 3669 } // else none are locked
3372 } // end Rotation Update 3670 } // end Rotation Update
3373 3671
3374 3672
3375 // VEHICLE processing ========================================== 3673 // VEHICLE processing ==========================================
3376 if (m_type != Vehicle.TYPE_NONE) 3674 if (m_type != Vehicle.TYPE_NONE)
3377 { 3675 {
3378 // get body attitude 3676 // get body attitude
3379 d.Quaternion rot = d.BodyGetQuaternion(Body); 3677 d.Quaternion rot = d.BodyGetQuaternion(Body);
3380 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object 3678 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
3381 Quaternion irotq = Quaternion.Inverse(rotq); 3679 Quaternion irotq = Quaternion.Inverse(rotq);
3382 3680
3383 // VEHICLE Linear Motion 3681 // VEHICLE Linear Motion
3384 d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame 3682 d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame
3385 Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z); 3683 Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z);
3386 m_lLinObjectVel = vel_now * irotq; 3684 m_lLinObjectVel = vel_now * irotq;
3387 if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate 3685 if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate
3388 { 3686 {
3389 if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f) 3687 if (Vector3.Mag(m_lLinMotorDVel) < 1.0f)
3390 { 3688 {
3391 float decayfactor = m_linearMotorDecayTimescale/timestep; 3689 float decayfactor = m_linearMotorDecayTimescale / timestep;
3392 Vector3 decayAmount = (m_lLinMotorDVel/decayfactor); 3690 Vector3 decayAmount = (m_lLinMotorDVel / decayfactor);
3393 m_lLinMotorDVel -= decayAmount; 3691 m_lLinMotorDVel -= decayAmount;
3394 } 3692 }
3395 else 3693 else
3396 { 3694 {
3397 float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale))); 3695 float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale)));
3398 Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep; 3696 Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep;
3399 m_lLinMotorDVel -= decel; 3697 m_lLinMotorDVel -= decel;
3400 } 3698 }
3401 if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) 3699 if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
3402 { 3700 {
3403 m_lLinMotorDVel = Vector3.Zero; 3701 m_lLinMotorDVel = Vector3.Zero;
3404 } 3702 }
3405 3703
3406 /* else 3704 /* else
3407 { 3705 {
3408 if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X; 3706 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; 3707 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; 3708 if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z;
3411 } */ 3709 } */
3412 } // end linear motor decay 3710 } // end linear motor decay
3413 3711
3414 if ( (! m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) ) 3712 if ((!m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (!m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)))
3415 { 3713 {
3416 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); 3714 if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body);
3417 if (m_linearMotorTimescale < 300.0f) 3715 if (m_linearMotorTimescale < 300.0f)
3418 { 3716 {
3419 Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel; 3717 Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel;
3420 float linfactor = m_linearMotorTimescale/timestep; 3718 float linfactor = m_linearMotorTimescale / timestep;
3421 Vector3 attackAmount = (attack_error/linfactor) * 1.3f; 3719 Vector3 attackAmount = (attack_error / linfactor) * 1.3f;
3422 m_lLinObjectVel += attackAmount; 3720 m_lLinObjectVel += attackAmount;
3423 } 3721 }
3424 if (m_linearFrictionTimescale.X < 300.0f) 3722 if (m_linearFrictionTimescale.X < 300.0f)
3425 { 3723 {
3426 float fricfactor = m_linearFrictionTimescale.X / timestep; 3724 float fricfactor = m_linearFrictionTimescale.X / timestep;
3427 float fricX = m_lLinObjectVel.X / fricfactor; 3725 float fricX = m_lLinObjectVel.X / fricfactor;
3428 m_lLinObjectVel.X -= fricX; 3726 m_lLinObjectVel.X -= fricX;
3429 } 3727 }
3430 if (m_linearFrictionTimescale.Y < 300.0f) 3728 if (m_linearFrictionTimescale.Y < 300.0f)
3431 { 3729 {
3432 float fricfactor = m_linearFrictionTimescale.Y / timestep; 3730 float fricfactor = m_linearFrictionTimescale.Y / timestep;
3433 float fricY = m_lLinObjectVel.Y / fricfactor; 3731 float fricY = m_lLinObjectVel.Y / fricfactor;
3434 m_lLinObjectVel.Y -= fricY; 3732 m_lLinObjectVel.Y -= fricY;
3435 } 3733 }
3436 if (m_linearFrictionTimescale.Z < 300.0f) 3734 if (m_linearFrictionTimescale.Z < 300.0f)
3437 { 3735 {
3438 float fricfactor = m_linearFrictionTimescale.Z / timestep; 3736 float fricfactor = m_linearFrictionTimescale.Z / timestep;
3439 float fricZ = m_lLinObjectVel.Z / fricfactor; 3737 float fricZ = m_lLinObjectVel.Z / fricfactor;
3440 m_lLinObjectVel.Z -= fricZ; 3738 m_lLinObjectVel.Z -= fricZ;
3441 } 3739 }
3442 } 3740 }
3443 m_wLinObjectVel = m_lLinObjectVel * rotq; 3741 m_wLinObjectVel = m_lLinObjectVel * rotq;
3444 3742
3445 // Gravity and Buoyancy 3743 // Gravity and Buoyancy
3446 Vector3 grav = Vector3.Zero; 3744 Vector3 grav = Vector3.Zero;
3447 if(m_VehicleBuoyancy < 1.0f) 3745 if (m_VehicleBuoyancy < 1.0f)
3448 { 3746 {
3449 // There is some gravity, make a gravity force vector 3747 // There is some gravity, make a gravity force vector
3450 // that is applied after object velocity. 3748 // that is applied after object velocity.
3451 d.Mass objMass; 3749 d.Mass objMass;
3452 d.BodyGetMass(Body, out objMass); 3750 d.BodyGetMass(Body, out objMass);
3453 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; 3751 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
3454 grav.Z = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force 3752 grav.Z = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force
3455 } // else its 1.0, no gravity. 3753 } // else its 1.0, no gravity.
3456 3754
3457 // Hovering 3755 // Hovering
3458 if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) 3756 if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
3459 { 3757 {
3460 // We should hover, get the target height 3758 // We should hover, get the target height
3461 d.Vector3 pos = d.BodyGetPosition(Body); 3759 d.Vector3 pos = d.BodyGetPosition(Body);
3462 if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY) 3760 if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
3463 { 3761 {
3464 m_VhoverTargetHeight = _parent_scene.GetWaterLevel() + m_VhoverHeight; 3762 m_VhoverTargetHeight = _parent_scene.GetWaterLevel() + m_VhoverHeight;
3465 } 3763 }
3466 else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY) 3764 else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
3467 { 3765 {
3468 m_VhoverTargetHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight; 3766 m_VhoverTargetHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
3469 } 3767 }
3470 else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT) 3768 else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
3471 { 3769 {
3472 m_VhoverTargetHeight = m_VhoverHeight; 3770 m_VhoverTargetHeight = m_VhoverHeight;
3473 } 3771 }
3474 3772
3475 if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY) 3773 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
3476 { 3774 {
3477 // If body is aready heigher, use its height as target height 3775 // If body is aready heigher, use its height as target height
3478 if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z; 3776 if (pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
3479 } 3777 }
3480 3778
3481// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped 3779 // m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
3482// m_VhoverTimescale = 0f; // time to acheive height 3780 // m_VhoverTimescale = 0f; // time to acheive height
3483// timestep is time since last frame,in secs 3781 // timestep is time since last frame,in secs
3484 float herr0 = pos.Z - m_VhoverTargetHeight; 3782 float herr0 = pos.Z - m_VhoverTargetHeight;
3485 // Replace Vertical speed with correction figure if significant 3783 // Replace Vertical speed with correction figure if significant
3486 if(Math.Abs(herr0) > 0.01f ) 3784 if (Math.Abs(herr0) > 0.01f)
3487 { 3785 {
3488 //? d.Mass objMass; 3786 //? d.Mass objMass;
3489 //? d.BodyGetMass(Body, out objMass); 3787 //? d.BodyGetMass(Body, out objMass);
3490 m_wLinObjectVel.Z = - ( (herr0 * timestep * 50.0f) / m_VhoverTimescale); 3788 m_wLinObjectVel.Z = -((herr0 * timestep * 50.0f) / m_VhoverTimescale);
3491 //KF: m_VhoverEfficiency is not yet implemented 3789 //KF: m_VhoverEfficiency is not yet implemented
3492 } 3790 }
3493 else 3791 else
3494 { 3792 {
3495 m_wLinObjectVel.Z = 0f; 3793 m_wLinObjectVel.Z = 0f;
3496 } 3794 }
3497 } 3795 }
3498 else 3796 else
3499 { // not hovering 3797 { // not hovering
3500 if (m_wLinObjectVel.Z == 0f) 3798 if (m_wLinObjectVel.Z == 0f)
3501 { // Gravity rules 3799 { // Gravity rules
3502 m_wLinObjectVel.Z = vel_now.Z; 3800 m_wLinObjectVel.Z = vel_now.Z;
3503 } // else the motor has it 3801 } // else the motor has it
3504 } 3802 }
3505 linvel = m_wLinObjectVel; 3803 linvel = m_wLinObjectVel;
3506 3804
3507 // Vehicle Linear Motion done ======================================= 3805 // Vehicle Linear Motion done =======================================
3508 // Apply velocity 3806 // Apply velocity
3509 d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z); 3807 d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z);
3510 // apply gravity force 3808 // apply gravity force
3511 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); 3809 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
3512//if(frcount == 0) Console.WriteLine("Vel={0} Force={1}",linvel , grav); 3810 //if(frcount == 0) Console.WriteLine("Vel={0} Force={1}",linvel , grav);
3513 // end MoveLinear() 3811 // end MoveLinear()
3514 3812
3515 3813
3516 // MoveAngular 3814 // MoveAngular
3517 /* 3815 /*
3518 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor 3816 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 3817
3529 d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body); 3818 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
3530 Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z); 3819 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
3531 angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation 3820 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
3532 3821
3533//if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel); 3822 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
3534 3823 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. 3824 */
3536 float atk_decayfactor = 23.0f / (m_angularMotorTimescale * timestep); 3825 //if(frcount == 0) Console.WriteLine("MoveAngular ");
3537 m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor; 3826
3538 // Decay Angular Motor 2. 3827 d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body);
3539 if (m_angularMotorDecayTimescale < 300.0f) 3828 Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z);
3540 { 3829 angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation
3541 if ( Vector3.Mag(m_angularMotorDVel) < 1.0f) 3830
3542 { 3831 //if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel);
3543 float decayfactor = (m_angularMotorDecayTimescale)/timestep; 3832
3544 Vector3 decayAmount = (m_angularMotorDVel/decayfactor); 3833 // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack.
3545 m_angularMotorDVel -= decayAmount; 3834 float atk_decayfactor = 23.0f / (m_angularMotorTimescale * timestep);
3546 } 3835 m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor;
3547 else 3836 // Decay Angular Motor 2.
3548 { 3837 if (m_angularMotorDecayTimescale < 300.0f)
3549 Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale; 3838 {
3550 m_angularMotorDVel -= decel; 3839 if (Vector3.Mag(m_angularMotorDVel) < 1.0f)
3551 } 3840 {
3552 3841 float decayfactor = (m_angularMotorDecayTimescale) / timestep;
3553 if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) 3842 Vector3 decayAmount = (m_angularMotorDVel / decayfactor);
3554 { 3843 m_angularMotorDVel -= decayAmount;
3555 m_angularMotorDVel = Vector3.Zero; 3844 }
3556 } 3845 else
3557 else 3846 {
3558 { 3847 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; 3848 m_angularMotorDVel -= decel;
3560 if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y; 3849 }
3561 if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z; 3850
3562 } 3851 if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
3563 } // end decay angular motor 3852 {
3564//if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel); 3853 m_angularMotorDVel = Vector3.Zero;
3565 3854 }
3566//if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel); 3855 else
3567 3856 {
3568 if ( (! m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) ) 3857 if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X;
3569 { // if motor or object have motion 3858 if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y;
3570 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); 3859 if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z;
3571 3860 }
3572 if (m_angularMotorTimescale < 300.0f) 3861 } // end decay angular motor
3573 { 3862 //if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel);
3574 Vector3 attack_error = m_angularMotorDVel - angObjectVel; 3863
3575 float angfactor = m_angularMotorTimescale/timestep; 3864 //if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel);
3576 Vector3 attackAmount = (attack_error/angfactor); 3865
3577 angObjectVel += attackAmount; 3866 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); 3867 { // if motor or object have motion
3579//if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel); 3868 if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body);
3580 } 3869
3581 3870 if (m_angularMotorTimescale < 300.0f)
3582 angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep); 3871 {
3583 angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep); 3872 Vector3 attack_error = m_angularMotorDVel - angObjectVel;
3584 angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep); 3873 float angfactor = m_angularMotorTimescale / timestep;
3585 } // else no signif. motion 3874 Vector3 attackAmount = (attack_error / angfactor);
3586 3875 angObjectVel += attackAmount;
3587//if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel); 3876 //if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount);
3588 // Bank section tba 3877 //if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel);
3589 // Deflection section tba 3878 }
3590//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel); 3879
3591 3880 angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep);
3592 3881 angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep);
3593 /* // Rotation Axis Disables: 3882 angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep);
3594 if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f)) 3883 } // else no signif. motion
3595 { 3884
3596 if (m_angularEnable.X == 0) 3885 //if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel);
3597 angObjectVel.X = 0f; 3886 // Bank section tba
3598 if (m_angularEnable.Y == 0) 3887 // Deflection section tba
3599 angObjectVel.Y = 0f; 3888 //if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel);
3600 if (m_angularEnable.Z == 0) 3889
3601 angObjectVel.Z = 0f; 3890
3602 } 3891 /* // Rotation Axis Disables:
3603 */ 3892 if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f))
3604 angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation 3893 {
3605 3894 if (m_angularEnable.X == 0)
3606 // Vertical attractor section 3895 angObjectVel.X = 0f;
3607 Vector3 vertattr = Vector3.Zero; 3896 if (m_angularEnable.Y == 0)
3608 3897 angObjectVel.Y = 0f;
3609 if(m_verticalAttractionTimescale < 300) 3898 if (m_angularEnable.Z == 0)
3610 { 3899 angObjectVel.Z = 0f;
3611 float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep); 3900 }
3612 // make a vector pointing up 3901 */
3613 Vector3 verterr = Vector3.Zero; 3902 angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation
3614 verterr.Z = 1.0f; 3903
3615 // rotate it to Body Angle 3904 // Vertical attractor section
3616 verterr = verterr * rotq; 3905 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. 3906
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 3907 if (m_verticalAttractionTimescale < 300)
3619 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. 3908 {
3620 3909 float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep);
3621 if (verterr.Z < 0.0f) 3910 // make a vector pointing up
3622 { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to 3911 Vector3 verterr = Vector3.Zero;
3623 // vertical, BUT for some reason a z-rotation is imparted to the object. TBI. 3912 verterr.Z = 1.0f;
3624//Console.WriteLine("InvertFlip"); 3913 // rotate it to Body Angle
3625 verterr.X = 2.0f - verterr.X; 3914 verterr = verterr * rotq;
3626 verterr.Y = 2.0f - verterr.Y; 3915 // 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 } 3916 // 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; 3917 // 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) 3918
3630 Vector3 xyav = angObjectVel; 3919 if (verterr.Z < 0.0f)
3631 xyav.Z = 0.0f; 3920 { // 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)) 3921 // vertical, BUT for some reason a z-rotation is imparted to the object. TBI.
3633 { 3922 //Console.WriteLine("InvertFlip");
3634 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so 3923 verterr.X = 2.0f - verterr.X;
3635 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. 3924 verterr.Y = 2.0f - verterr.Y;
3636 vertattr.X = verterr.Y; 3925 }
3637 vertattr.Y = - verterr.X; 3926 verterr *= 0.5f;
3638 vertattr.Z = 0f; 3927 // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt)
3639//if(frcount == 0) Console.WriteLine("VAerr=" + verterr); 3928 Vector3 xyav = angObjectVel;
3640 3929 xyav.Z = 0.0f;
3641 // scaling appears better usingsquare-law 3930 if ((!xyav.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f))
3642 float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency; 3931 {
3643 float bounce = 1.0f - damped; 3932 // 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 3933 // 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 3934 vertattr.X = verterr.Y;
3646 // time-scaled correction, which sums, therefore is bouncy: 3935 vertattr.Y = -verterr.X;
3647 angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce; 3936 vertattr.Z = 0f;
3648 // damped, good @ < 90: 3937 //if(frcount == 0) Console.WriteLine("VAerr=" + verterr);
3649 angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped); 3938
3650 angObjectVel.Z = oavz; 3939 // scaling appears better usingsquare-law
3651//if(frcount == 0) Console.WriteLine("VA+"); 3940 float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
3652//Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel); 3941 float bounce = 1.0f - damped;
3653 } 3942 // 0 = crit damp, 1 = bouncy
3654 else 3943 float oavz = angObjectVel.Z; // retain z velocity
3655 { 3944 // time-scaled correction, which sums, therefore is bouncy:
3656 // else error is very small 3945 angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce;
3657 angObjectVel.X = 0f; 3946 // damped, good @ < 90:
3658 angObjectVel.Y = 0f; 3947 angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped);
3659//if(frcount == 0) Console.WriteLine("VA0"); 3948 angObjectVel.Z = oavz;
3660 } 3949 //if(frcount == 0) Console.WriteLine("VA+");
3661 } // else vertical attractor is off 3950 //Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel);
3662//if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel); 3951 }
3663 3952 else
3664 3953 {
3665 m_lastAngularVelocity = angObjectVel; 3954 // else error is very small
3666 // apply Angular Velocity to body 3955 angObjectVel.X = 0f;
3667 d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); 3956 angObjectVel.Y = 0f;
3668//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity); 3957 //if(frcount == 0) Console.WriteLine("VA0");
3669 3958 }
3670 } // end VEHICLES 3959 } // else vertical attractor is off
3671 else 3960 //if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel);
3672 { 3961
3673 // Dyamics (NON-'VEHICLES') are dealt with here ================================================================ 3962
3674 3963 m_lastAngularVelocity = angObjectVel;
3675 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 3964 // apply Angular Velocity to body
3676 3965 d.BodySetAngularVel(Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
3677 /// Dynamics Buoyancy 3966 //if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity);
3678 //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle. 3967
3679 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up 3968 } // end VEHICLES
3680 // NB Prims in ODE are no subject to global gravity 3969 else
3681 // This should only affect gravity operations 3970 {
3682 3971 // Dyamics (NON-'VEHICLES') are dealt with here ================================================================
3683 float m_mass = CalculateMass(); 3972
3684 // calculate z-force due togravity on object. 3973 if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009
3685 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass 3974
3686 if ((m_usePID) && (m_PIDTau > 0.0f)) // Dynamics llMoveToTarget. 3975 /// Dynamics Buoyancy
3687 { 3976 //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle.
3688 fz = 0; // llMoveToTarget ignores gravity. 3977 // 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. 3978 // NB Prims in ODE are no subject to global gravity
3690 // Vector3 m_PIDTarget is where we are going 3979 // This should only affect gravity operations
3691 // float m_PIDTau is time to get there 3980
3981 float m_mass = CalculateMass();
3982 // calculate z-force due togravity on object.
3983 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass
3984 if ((m_usePID) && (m_PIDTau > 0.0f)) // Dynamics llMoveToTarget.
3985 {
3986 fz = 0; // llMoveToTarget ignores gravity.
3987 // it also ignores mass of object, and any physical resting on it.
3988 // Vector3 m_PIDTarget is where we are going
3989 // float m_PIDTau is time to get there
3692 fx = 0; 3990 fx = 0;
3693 fy = 0; 3991 fy = 0;
3694 d.Vector3 pos = d.BodyGetPosition(Body); 3992 d.Vector3 pos = d.BodyGetPosition(Body);
3695 Vector3 error = new Vector3( 3993 Vector3 error = new Vector3(
3696 (m_PIDTarget.X - pos.X), 3994 (m_PIDTarget.X - pos.X),
3697 (m_PIDTarget.Y - pos.Y), 3995 (m_PIDTarget.Y - pos.Y),
3698 (m_PIDTarget.Z - pos.Z)); 3996 (m_PIDTarget.Z - pos.Z));
3699 if (error.ApproxEquals(Vector3.Zero,0.01f)) 3997 if (error.ApproxEquals(Vector3.Zero, 0.01f))
3700 { // Very close, Jump there and quit move 3998 { // Very close, Jump there and quit move
3701 3999
3702 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); 4000 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3703 _target_velocity = Vector3.Zero; 4001 _target_velocity = Vector3.Zero;
3704 d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); 4002 d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
3705 d.BodySetForce(Body, 0f, 0f, 0f); 4003 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 } 4004 }
3720 } // end PID MoveToTarget 4005 else
3721 4006 {
3722 4007 float scale = 50.0f * timestep / m_PIDTau;
3723 /// Dynamics Hover =================================================================================== 4008 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. 4009 {
3725 if (m_useHoverPID && !m_usePID) 4010 // Nearby, quit update of velocity
3726 { 4011 }
3727//Console.WriteLine("Hover " + m_primName); 4012 else
3728 4013 { // Far, calc damped velocity
3729 // If we're using the PID controller, then we have no gravity 4014 _target_velocity = error * scale;
3730 fz = (-1 * _parent_scene.gravityz) * m_mass; 4015 }
3731 4016 d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
3732 // no lock; for now it's only called from within Simulate() 4017 }
3733 4018 } // end PID MoveToTarget
3734 // If the PID Controller isn't active then we set our force 4019
3735 // calculating base velocity to the current position 4020
3736 4021 /// Dynamics Hover ===================================================================================
3737 if ((m_PIDTau < 1)) 4022 // Hover PID Controller can only run if the PIDcontroller is not in use.
3738 { 4023 if (m_useHoverPID && !m_usePID)
3739 PID_G = PID_G / m_PIDTau; 4024 {
3740 } 4025 //Console.WriteLine("Hover " + m_primName);
3741 4026
3742 if ((PID_G - m_PIDTau) <= 0) 4027 // If we're using the PID controller, then we have no gravity
3743 { 4028 fz = (-1 * _parent_scene.gravityz) * m_mass;
3744 PID_G = m_PIDTau + 1; 4029
3745 } 4030 // no lock; for now it's only called from within Simulate()
3746 4031
3747 4032 // If the PID Controller isn't active then we set our force
3748 // Where are we, and where are we headed? 4033 // calculating base velocity to the current position
3749 d.Vector3 pos = d.BodyGetPosition(Body); 4034
3750// d.Vector3 vel = d.BodyGetLinearVel(Body); 4035 if ((m_PIDTau < 1))
3751 4036 {
3752 4037 PID_G = PID_G / m_PIDTau;
3753 // Non-Vehicles have a limited set of Hover options. 4038 }
3754 // determine what our target height really is based on HoverType 4039
3755 switch (m_PIDHoverType) 4040 if ((PID_G - m_PIDTau) <= 0)
3756 { 4041 {
3757 case PIDHoverType.Ground: 4042 PID_G = m_PIDTau + 1;
3758 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); 4043 }
3759 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; 4044
3760 break; 4045
3761 case PIDHoverType.GroundAndWater: 4046 // Where are we, and where are we headed?
3762 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); 4047 d.Vector3 pos = d.BodyGetPosition(Body);
3763 m_waterHeight = _parent_scene.GetWaterLevel(); 4048 // d.Vector3 vel = d.BodyGetLinearVel(Body);
3764 if (m_groundHeight > m_waterHeight) 4049
3765 { 4050
3766 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; 4051 // Non-Vehicles have a limited set of Hover options.
3767 } 4052 // determine what our target height really is based on HoverType
3768 else 4053 switch (m_PIDHoverType)
3769 { 4054 {
3770 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; 4055 case PIDHoverType.Ground:
3771 } 4056 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3772 break; 4057 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3773 4058 break;
3774 } // end switch (m_PIDHoverType) 4059 case PIDHoverType.GroundAndWater:
3775 4060 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3776 4061 m_waterHeight = _parent_scene.GetWaterLevel();
3777 _target_velocity = 4062 if (m_groundHeight > m_waterHeight)
4063 {
4064 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
4065 }
4066 else
4067 {
4068 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
4069 }
4070 break;
4071
4072 } // end switch (m_PIDHoverType)
4073
4074
4075 _target_velocity =
3778 new Vector3(0.0f, 0.0f, 4076 new Vector3(0.0f, 0.0f,
3779 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) 4077 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
3780 ); 4078 );
3781 4079
3782 // if velocity is zero, use position control; otherwise, velocity control 4080 // if velocity is zero, use position control; otherwise, velocity control
3783 4081
3784 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) 4082 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
3785 { 4083 {
3786 // keep track of where we stopped. No more slippin' & slidin' 4084 // keep track of where we stopped. No more slippin' & slidin'
3787 4085
3788 // We only want to deactivate the PID Controller if we think we want to have our surrogate 4086 // 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. 4087 // react to the physics scene by moving it's position.
3790 // Avatar to Avatar collisions 4088 // Avatar to Avatar collisions
3791 // Prim to avatar collisions 4089 // Prim to avatar collisions
3792 d.Vector3 dlinvel = vel; 4090 d.Vector3 dlinvel = vel;
3793 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); 4091 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
3794 d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z); 4092 d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z);
3795 d.BodyAddForce(Body, 0, 0, fz); 4093 d.BodyAddForce(Body, 0, 0, fz);
3796 //KF this prevents furthur motions return; 4094 //KF this prevents furthur motions return;
3797 } 4095 }
3798 else 4096 else
3799 { 4097 {
3800 _zeroFlag = false; 4098 _zeroFlag = false;
3801 4099
3802 // We're flying and colliding with something 4100 // We're flying and colliding with something
3803 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); 4101 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
3804 } 4102 }
3805 } // end m_useHoverPID && !m_usePID 4103 } // end m_useHoverPID && !m_usePID
3806 4104
3807 4105
3808 /// Dynamics Apply Forces =================================================================================== 4106 /// Dynamics Apply Forces ===================================================================================
3809 fx *= m_mass; 4107 fx *= m_mass;
3810 fy *= m_mass; 4108 fy *= m_mass;
3811 //fz *= m_mass; 4109 //fz *= m_mass;
3812 fx += m_force.X; 4110 fx += m_force.X;
3813 fy += m_force.Y; 4111 fy += m_force.Y;
3814 fz += m_force.Z; 4112 fz += m_force.Z;
3815 4113
3816 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); 4114 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
3817 if (fx != 0 || fy != 0 || fz != 0) 4115 if (fx != 0 || fy != 0 || fz != 0)
3818 { 4116 {
3819 //m_taintdisable = true; 4117 //m_taintdisable = true;
3820 //base.RaiseOutOfBounds(Position); 4118 //base.RaiseOutOfBounds(Position);
3821 //d.BodySetLinearVel(Body, fx, fy, 0f); 4119 //d.BodySetLinearVel(Body, fx, fy, 0f);
3822 if (!d.BodyIsEnabled(Body)) 4120 if (!d.BodyIsEnabled(Body))
3823 { 4121 {
3824 // A physical body at rest on a surface will auto-disable after a while, 4122 // 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, 4123 // this appears to re-enable it incase the surface it is upon vanishes,
3826 // and the body should fall again. 4124 // and the body should fall again.
3827 d.BodySetLinearVel(Body, 0f, 0f, 0f); 4125 d.BodySetLinearVel(Body, 0f, 0f, 0f);
3828 d.BodySetForce(Body, 0f, 0f, 0f); 4126 d.BodySetForce(Body, 0f, 0f, 0f);
3829 enableBodySoft(); 4127 enableBodySoft();
3830 } 4128 }
3831 4129
3832 // 35x10 = 350n times the mass per second applied maximum. 4130 // 35x10 = 350n times the mass per second applied maximum.
3833 float nmax = 35f * m_mass; 4131 float nmax = 35f * m_mass;
3834 float nmin = -35f * m_mass; 4132 float nmin = -35f * m_mass;
3835 4133
3836 4134
3837 if (fx > nmax) 4135 if (fx > nmax)
3838 fx = nmax; 4136 fx = nmax;
3839 if (fx < nmin) 4137 if (fx < nmin)
3840 fx = nmin; 4138 fx = nmin;
3841 if (fy > nmax) 4139 if (fy > nmax)
3842 fy = nmax; 4140 fy = nmax;
3843 if (fy < nmin) 4141 if (fy < nmin)
3844 fy = nmin; 4142 fy = nmin;
3845 d.BodyAddForce(Body, fx, fy, fz); 4143 d.BodyAddForce(Body, fx, fy, fz);
3846 } // end apply forces 4144 } // end apply forces
3847 } // end Vehicle/Dynamics 4145 } // end Vehicle/Dynamics
3848 4146
3849 /// RotLookAt / LookAt ================================================================================= 4147 /// RotLookAt / LookAt =================================================================================
3850 if (m_useAPID) 4148 if (m_useAPID)
3851 { 4149 {
3852 // RotLookAt, apparently overrides all other rotation sources. Inputs: 4150 // RotLookAt, apparently overrides all other rotation sources. Inputs:
3853 // Quaternion m_APIDTarget 4151 // Quaternion m_APIDTarget
3854 // float m_APIDStrength // From SL experiments, this is the time to get there 4152 // 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 4153 // 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. 4154 // Also in SL the mass of the object has no effect on time to get there.
3857 // Factors: 4155 // Factors:
3858 // get present body rotation 4156 // get present body rotation
3859 float limit = 1.0f; 4157 float limit = 1.0f;
3860 float rscaler = 50f; // adjusts rotation damping time 4158 float rscaler = 50f; // adjusts rotation damping time
3861 float lscaler = 10f; // adjusts linear damping time in llLookAt 4159 float lscaler = 10f; // adjusts linear damping time in llLookAt
3862 float RLAservo = 0f; 4160 float RLAservo = 0f;
3863 Vector3 diff_axis; 4161 Vector3 diff_axis;
3864 float diff_angle; 4162 float diff_angle;
3865 d.Quaternion rot = d.BodyGetQuaternion(Body); // prim present rotation 4163 d.Quaternion rot = d.BodyGetQuaternion(Body); // prim present rotation
3866 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); 4164 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
3867 Quaternion rtarget = new Quaternion(); 4165 Quaternion rtarget = new Quaternion();
3868 4166
3869 if(m_APIDTarget.W == -99.9f) 4167 if (m_APIDTarget.W == -99.9f)
3870 { 4168 {
3871 // this is really a llLookAt(), x,y,z is the target vector 4169 // 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); 4170 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; 4171 Vector3 ospin = new Vector3(1.0f, 0.0f, 0.0f) * rotq;
3874 Vector3 error = new Vector3(0.0f, 0.0f, 0.0f); 4172 Vector3 error = new Vector3(0.0f, 0.0f, 0.0f);
3875 float twopi = 2.0f * (float)Math.PI; 4173 float twopi = 2.0f * (float)Math.PI;
3876 Vector3 dir = target - _position; 4174 Vector3 dir = target - _position;
3877 dir.Normalize(); 4175 dir.Normalize();
3878 float tzrot = (float)Math.Atan2(dir.Y, dir.X); 4176 float tzrot = (float)Math.Atan2(dir.Y, dir.X);
3879 float txy = (float)Math.Sqrt((dir.X * dir.X) + (dir.Y * dir.Y)); 4177 float txy = (float)Math.Sqrt((dir.X * dir.X) + (dir.Y * dir.Y));
3880 float terot = (float)Math.Atan2(dir.Z, txy); 4178 float terot = (float)Math.Atan2(dir.Z, txy);
@@ -3882,63 +4180,63 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
3882 float oxy = (float)Math.Sqrt((ospin.X * ospin.X) + (ospin.Y * ospin.Y)); 4180 float oxy = (float)Math.Sqrt((ospin.X * ospin.X) + (ospin.Y * ospin.Y));
3883 float oerot = (float)Math.Atan2(ospin.Z, oxy); 4181 float oerot = (float)Math.Atan2(ospin.Z, oxy);
3884 float ra = 2.0f * ((rotq.W * rotq.X) + (rotq.Y * rotq.Z)); 4182 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)); 4183 float rb = 1.0f - 2.0f * ((rotq.Y * rotq.Y) + (rotq.X * rotq.X));
3886 float roll = (float)Math.Atan2(ra, rb); 4184 float roll = (float)Math.Atan2(ra, rb);
3887 float errorz = tzrot - ozrot; 4185 float errorz = tzrot - ozrot;
3888 if(errorz > (float)Math.PI) errorz -= twopi; 4186 if (errorz > (float)Math.PI) errorz -= twopi;
3889 else if(errorz < -(float)Math.PI) errorz += twopi; 4187 else if (errorz < -(float)Math.PI) errorz += twopi;
3890 float errory = oerot - terot; 4188 float errory = oerot - terot;
3891 if(errory > (float)Math.PI) errory -= twopi; 4189 if (errory > (float)Math.PI) errory -= twopi;
3892 else if(errory < -(float)Math.PI) errory += twopi; 4190 else if (errory < -(float)Math.PI) errory += twopi;
3893 diff_angle = Math.Abs(errorz) + Math.Abs(errory) + Math.Abs(roll); 4191 diff_angle = Math.Abs(errorz) + Math.Abs(errory) + Math.Abs(roll);
3894 if(diff_angle > 0.01f * m_APIDdamper) 4192 if (diff_angle > 0.01f * m_APIDdamper)
3895 { 4193 {
3896 m_APIDdamper = 1.0f; 4194 m_APIDdamper = 1.0f;
3897 RLAservo = timestep / m_APIDStrength * rscaler; 4195 RLAservo = timestep / m_APIDStrength * rscaler;
3898 errorz *= RLAservo; 4196 errorz *= RLAservo;
3899 errory *= RLAservo; 4197 errory *= RLAservo;
3900 error.X = -roll * 8.0f; 4198 error.X = -roll * 8.0f;
3901 error.Y = errory; 4199 error.Y = errory;
3902 error.Z = errorz; 4200 error.Z = errorz;
3903 error *= rotq; 4201 error *= rotq;
3904 d.BodySetAngularVel (Body, error.X, error.Y, error.Z); 4202 d.BodySetAngularVel(Body, error.X, error.Y, error.Z);
3905 } 4203 }
3906 else 4204 else
3907 { 4205 {
3908 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); 4206 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
3909 m_APIDdamper = 2.0f; 4207 m_APIDdamper = 2.0f;
3910 } 4208 }
3911 } 4209 }
3912 else 4210 else
3913 { 4211 {
3914 // this is a llRotLookAt() 4212 // this is a llRotLookAt()
3915 rtarget = m_APIDTarget; 4213 rtarget = m_APIDTarget;
3916 4214
3917 Quaternion rot_diff = Quaternion.Inverse(rotq) * rtarget; // difference to desired rot 4215 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 4216 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); 4217 //if(frcount == 0) Console.WriteLine("axis {0} angle {1}",diff_axis * 57.3f, diff_angle);
3920 4218
3921 // diff_axis.Normalize(); it already is! 4219 // diff_axis.Normalize(); it already is!
3922 if(diff_angle > 0.01f * m_APIDdamper) // diff_angle is always +ve // if there is enough error 4220 if (diff_angle > 0.01f * m_APIDdamper) // diff_angle is always +ve // if there is enough error
3923 { 4221 {
3924 m_APIDdamper = 1.0f; 4222 m_APIDdamper = 1.0f;
3925 Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z); 4223 Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
3926 rotforce = rotforce * rotq; 4224 rotforce = rotforce * rotq;
3927 if(diff_angle > limit) diff_angle = limit; // cap the rotate rate 4225 if (diff_angle > limit) diff_angle = limit; // cap the rotate rate
3928 RLAservo = timestep / m_APIDStrength * lscaler; 4226 RLAservo = timestep / m_APIDStrength * lscaler;
3929 rotforce = rotforce * RLAservo * diff_angle ; 4227 rotforce = rotforce * RLAservo * diff_angle;
3930 d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z); 4228 d.BodySetAngularVel(Body, rotforce.X, rotforce.Y, rotforce.Z);
3931//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); 4229 //Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo);
3932 } 4230 }
3933 else 4231 else
3934 { // close enough 4232 { // close enough
3935 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); 4233 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
3936 m_APIDdamper = 2.0f; 4234 m_APIDdamper = 2.0f;
3937 } 4235 }
3938 } // end llLookAt/llRotLookAt 4236 } // end llLookAt/llRotLookAt
3939//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle); 4237 //if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle);
3940 } // end m_useAPID 4238 } // end m_useAPID
3941 } // end root prims 4239 } // end root prims
3942 } // end Move() 4240 } // end Move()
3943 } // end class 4241 } // end class
3944} 4242} \ No newline at end of file
generated by cgit v1.1 at 2024-12-22 13:26:23 +0000