aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs')
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs187
1 files changed, 101 insertions, 86 deletions
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
index ef2dccc..9e145ec 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
@@ -82,13 +82,6 @@ namespace OpenSim.Region.Physics.OdePlugin
82 private IntPtr m_body = IntPtr.Zero; 82 private IntPtr m_body = IntPtr.Zero;
83// private IntPtr m_jointGroup = IntPtr.Zero; 83// private IntPtr m_jointGroup = IntPtr.Zero;
84// private IntPtr m_aMotor = IntPtr.Zero; 84// private IntPtr m_aMotor = IntPtr.Zero;
85
86 // Correction factors, to match Sl
87 private static float m_linearVelocityFactor = 0.9f;
88 private static float m_linearAttackFactor = 0.4f;
89 private static float m_linearDecayFactor = 0.5f;
90 private static float m_linearFrictionFactor = 1.2f;
91
92 85
93 // Vehicle properties 86 // Vehicle properties
94 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind 87 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
@@ -103,15 +96,15 @@ namespace OpenSim.Region.Physics.OdePlugin
103 // LIMIT_ROLL_ONLY 96 // LIMIT_ROLL_ONLY
104 97
105 // Linear properties 98 // Linear properties
106 private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time 99 private Vector3 m_linearMotorDirection = Vector3.Zero; // (was m_linearMotorDirectionLASTSET) the (local) Velocity
107 private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL, for max limiting 100 //requested by LSL
108 private Vector3 m_dir = Vector3.Zero; // velocity applied to body 101 private float m_linearMotorTimescale = 0; // Motor Attack rate set by LSL
109 private Vector3 m_linearFrictionTimescale = Vector3.Zero; 102 private float m_linearMotorDecayTimescale = 0; // Motor Decay rate set by LSL
110 private float m_linearMotorDecayTimescale = 0; 103 private Vector3 m_linearFrictionTimescale = Vector3.Zero; // General Friction set by LSL
111 private float m_linearMotorTimescale = 0; 104
112 private Vector3 m_lastLinearVelocityVector = Vector3.Zero; 105 private Vector3 m_lLinMotorDVel = Vector3.Zero; // decayed motor
113 // private bool m_LinearMotorSetLastFrame = false; 106 private Vector3 m_lLinObjectVel = Vector3.Zero; // local frame object velocity
114 // private Vector3 m_linearMotorOffset = Vector3.Zero; 107 private Vector3 m_wLinObjectVel = Vector3.Zero; // world frame object velocity
115 108
116 //Angular properties 109 //Angular properties
117 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor 110 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
@@ -241,7 +234,7 @@ namespace OpenSim.Region.Physics.OdePlugin
241 break; 234 break;
242 case Vehicle.LINEAR_MOTOR_DIRECTION: 235 case Vehicle.LINEAR_MOTOR_DIRECTION:
243 m_linearMotorDirection = new Vector3(pValue, pValue, pValue); 236 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
244 m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue); 237 UpdateLinDecay();
245 break; 238 break;
246 case Vehicle.LINEAR_MOTOR_OFFSET: 239 case Vehicle.LINEAR_MOTOR_OFFSET:
247 // m_linearMotorOffset = new Vector3(pValue, pValue, pValue); 240 // m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
@@ -273,9 +266,8 @@ namespace OpenSim.Region.Physics.OdePlugin
273 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); 266 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
274 break; 267 break;
275 case Vehicle.LINEAR_MOTOR_DIRECTION: 268 case Vehicle.LINEAR_MOTOR_DIRECTION:
276 pValue *= m_linearVelocityFactor; 269 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); // velocity requested by LSL, for max limiting
277 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); // velocity requested by LSL, decayed by time 270 UpdateLinDecay();
278 m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); // velocity requested by LSL, for max limiting
279 break; 271 break;
280 case Vehicle.LINEAR_MOTOR_OFFSET: 272 case Vehicle.LINEAR_MOTOR_OFFSET:
281 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); 273 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
@@ -304,7 +296,7 @@ namespace OpenSim.Region.Physics.OdePlugin
304 case Vehicle.TYPE_SLED: 296 case Vehicle.TYPE_SLED:
305 m_linearFrictionTimescale = new Vector3(30, 1, 1000); 297 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
306 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); 298 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
307 m_linearMotorDirection = Vector3.Zero; 299// m_lLinMotorVel = Vector3.Zero;
308 m_linearMotorTimescale = 1000; 300 m_linearMotorTimescale = 1000;
309 m_linearMotorDecayTimescale = 120; 301 m_linearMotorDecayTimescale = 120;
310 m_angularMotorDirection = Vector3.Zero; 302 m_angularMotorDirection = Vector3.Zero;
@@ -330,7 +322,7 @@ namespace OpenSim.Region.Physics.OdePlugin
330 case Vehicle.TYPE_CAR: 322 case Vehicle.TYPE_CAR:
331 m_linearFrictionTimescale = new Vector3(100, 2, 1000); 323 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
332 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); 324 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
333 m_linearMotorDirection = Vector3.Zero; 325// m_lLinMotorVel = Vector3.Zero;
334 m_linearMotorTimescale = 1; 326 m_linearMotorTimescale = 1;
335 m_linearMotorDecayTimescale = 60; 327 m_linearMotorDecayTimescale = 60;
336 m_angularMotorDirection = Vector3.Zero; 328 m_angularMotorDirection = Vector3.Zero;
@@ -357,7 +349,7 @@ namespace OpenSim.Region.Physics.OdePlugin
357 case Vehicle.TYPE_BOAT: 349 case Vehicle.TYPE_BOAT:
358 m_linearFrictionTimescale = new Vector3(10, 3, 2); 350 m_linearFrictionTimescale = new Vector3(10, 3, 2);
359 m_angularFrictionTimescale = new Vector3(10,10,10); 351 m_angularFrictionTimescale = new Vector3(10,10,10);
360 m_linearMotorDirection = Vector3.Zero; 352// m_lLinMotorVel = Vector3.Zero;
361 m_linearMotorTimescale = 5; 353 m_linearMotorTimescale = 5;
362 m_linearMotorDecayTimescale = 60; 354 m_linearMotorDecayTimescale = 60;
363 m_angularMotorDirection = Vector3.Zero; 355 m_angularMotorDirection = Vector3.Zero;
@@ -385,7 +377,7 @@ namespace OpenSim.Region.Physics.OdePlugin
385 case Vehicle.TYPE_AIRPLANE: 377 case Vehicle.TYPE_AIRPLANE:
386 m_linearFrictionTimescale = new Vector3(200, 10, 5); 378 m_linearFrictionTimescale = new Vector3(200, 10, 5);
387 m_angularFrictionTimescale = new Vector3(20, 20, 20); 379 m_angularFrictionTimescale = new Vector3(20, 20, 20);
388 m_linearMotorDirection = Vector3.Zero; 380// m_lLinMotorVel = Vector3.Zero;
389 m_linearMotorTimescale = 2; 381 m_linearMotorTimescale = 2;
390 m_linearMotorDecayTimescale = 60; 382 m_linearMotorDecayTimescale = 60;
391 m_angularMotorDirection = Vector3.Zero; 383 m_angularMotorDirection = Vector3.Zero;
@@ -412,7 +404,6 @@ namespace OpenSim.Region.Physics.OdePlugin
412 case Vehicle.TYPE_BALLOON: 404 case Vehicle.TYPE_BALLOON:
413 m_linearFrictionTimescale = new Vector3(5, 5, 5); 405 m_linearFrictionTimescale = new Vector3(5, 5, 5);
414 m_angularFrictionTimescale = new Vector3(10, 10, 10); 406 m_angularFrictionTimescale = new Vector3(10, 10, 10);
415 m_linearMotorDirection = Vector3.Zero;
416 m_linearMotorTimescale = 5; 407 m_linearMotorTimescale = 5;
417 m_linearMotorDecayTimescale = 60; 408 m_linearMotorDecayTimescale = 60;
418 m_angularMotorDirection = Vector3.Zero; 409 m_angularMotorDirection = Vector3.Zero;
@@ -453,7 +444,7 @@ namespace OpenSim.Region.Physics.OdePlugin
453 if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE) 444 if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
454 return; 445 return;
455 frcount++; // used to limit debug comment output 446 frcount++; // used to limit debug comment output
456 if (frcount > 100) 447 if (frcount > 24)
457 frcount = 0; 448 frcount = 0;
458 449
459 MoveLinear(pTimestep, pParentScene); 450 MoveLinear(pTimestep, pParentScene);
@@ -463,63 +454,90 @@ namespace OpenSim.Region.Physics.OdePlugin
463 internal void Halt() 454 internal void Halt()
464 { // Kill all motions, when non-physical 455 { // Kill all motions, when non-physical
465 m_linearMotorDirection = Vector3.Zero; 456 m_linearMotorDirection = Vector3.Zero;
466 m_linearMotorDirectionLASTSET = Vector3.Zero; 457 m_lLinMotorDVel = Vector3.Zero;
467 m_dir = Vector3.Zero; 458 m_lLinObjectVel = Vector3.Zero;
468 m_lastLinearVelocityVector = Vector3.Zero; 459 m_wLinObjectVel = Vector3.Zero;
469 m_angularMotorDirection = Vector3.Zero; 460 m_angularMotorDirection = Vector3.Zero;
470 m_angularMotorVelocity = Vector3.Zero; 461 m_angularMotorVelocity = Vector3.Zero;
471 m_lastAngularVelocity = Vector3.Zero; 462 m_lastAngularVelocity = Vector3.Zero;
472 } 463 }
464
465 private void UpdateLinDecay()
466 {
467 if (Math.Abs(m_linearMotorDirection.X) > Math.Abs(m_lLinMotorDVel.X)) m_lLinMotorDVel.X = m_linearMotorDirection.X;
468 if (Math.Abs(m_linearMotorDirection.Y) > Math.Abs(m_lLinMotorDVel.Y)) m_lLinMotorDVel.Y = m_linearMotorDirection.Y;
469 if (Math.Abs(m_linearMotorDirection.Z) > Math.Abs(m_lLinMotorDVel.Z)) m_lLinMotorDVel.Z = m_linearMotorDirection.Z;
470 } // else let the motor decay on its own
473 471
474 private void MoveLinear(float pTimestep, OdeScene _pParentScene) 472 private void MoveLinear(float pTimestep, OdeScene _pParentScene)
475 { 473 {
476 if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant 474 Vector3 acceleration = new Vector3(0f, 0f, 0f);
477 {
478 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
479 475
480 // add drive to body
481 float linfactor = m_linearMotorTimescale/pTimestep;
482 // Linear accel
483 Vector3 addAmount1 = (m_linearMotorDirection/linfactor) * 0.8f;
484 // Differential accel
485 Vector3 addAmount2 = ((m_linearMotorDirection - m_lastLinearVelocityVector)/linfactor) * 1.6f;
486 // SL correction
487 Vector3 addAmount = (addAmount1 + addAmount2) * m_linearAttackFactor;
488 m_lastLinearVelocityVector += addAmount; // lastLinearVelocityVector is the current body velocity vector
489//if(frcount == 0) Console.WriteLine("AL {0} + AD {1} AS{2} V {3}", addAmount1, addAmount2, addAmount, m_lastLinearVelocityVector);
490 // This will work temporarily, but we really need to compare speed on an axis
491 // KF: Limit body velocity to applied velocity?
492 if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X))
493 m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X;
494 if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y))
495 m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y;
496 if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z))
497 m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z;
498
499 // decay applied velocity
500 Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep)));
501 //Console.WriteLine("decay: " + decayfraction);
502 m_linearMotorDirection -= m_linearMotorDirection * decayfraction * m_linearDecayFactor;
503 //Console.WriteLine("actual: " + m_linearMotorDirection);
504 }
505 else
506 { // requested is not significant
507 // if what remains of applied is small, zero it.
508 if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
509 m_lastLinearVelocityVector = Vector3.Zero;
510 }
511
512
513 // convert requested object velocity to world-referenced vector
514 m_dir = m_lastLinearVelocityVector;
515 d.Quaternion rot = d.BodyGetQuaternion(Body); 476 d.Quaternion rot = d.BodyGetQuaternion(Body);
516 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object 477 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
517 m_dir *= rotq; // apply obj rotation to velocity vector 478 Quaternion irotq = Quaternion.Inverse(rotq);
479 d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame
480 Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z);
481 acceleration = vel_now - m_wLinObjectVel;
482 m_lLinObjectVel = vel_now * irotq;
483
484 if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate
485 {
486 if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f)
487 {
488 float decayfactor = m_linearMotorDecayTimescale/pTimestep;
489 Vector3 decayAmount = (m_lLinMotorDVel/decayfactor);
490 m_lLinMotorDVel -= decayAmount;
491 }
492 else
493 {
494 float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale)));
495 Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * pTimestep;
496 m_lLinMotorDVel -= decel;
497 }
498 if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
499 {
500 m_lLinMotorDVel = Vector3.Zero;
501 }
502 else
503 {
504 if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X;
505 if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y;
506 if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z;
507 }
508 }
518 509
519 // add Gravity and Buoyancy 510 if ( (! m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
520 // KF: So far I have found no good method to combine a script-requested 511 {
521 // .Z velocity and gravity. Therefore only 0g will used script-requested 512 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
522 // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only. 513 if (m_linearMotorTimescale < 300.0f)
514 {
515 Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel;
516 float linfactor = m_linearMotorTimescale/pTimestep;
517 Vector3 attackAmount = (attack_error/linfactor) * 1.3f;
518 m_lLinObjectVel += attackAmount;
519 }
520 if (m_linearFrictionTimescale.X < 300.0f)
521 {
522 float fricfactor = m_linearFrictionTimescale.X / pTimestep;
523 float fricX = m_lLinObjectVel.X / fricfactor;
524 m_lLinObjectVel.X -= fricX;
525 }
526 if (m_linearFrictionTimescale.Y < 300.0f)
527 {
528 float fricfactor = m_linearFrictionTimescale.Y / pTimestep;
529 float fricY = m_lLinObjectVel.Y / fricfactor;
530 m_lLinObjectVel.Y -= fricY;
531 }
532 if (m_linearFrictionTimescale.Z < 300.0f)
533 {
534 float fricfactor = m_linearFrictionTimescale.Z / pTimestep;
535 float fricZ = m_lLinObjectVel.Z / fricfactor;
536 m_lLinObjectVel.Z -= fricZ;
537 }
538 }
539 m_wLinObjectVel = m_lLinObjectVel * rotq;
540 // Add Gravity and Buoyancy
523 Vector3 grav = Vector3.Zero; 541 Vector3 grav = Vector3.Zero;
524 if(m_VehicleBuoyancy < 1.0f) 542 if(m_VehicleBuoyancy < 1.0f)
525 { 543 {
@@ -528,10 +546,7 @@ namespace OpenSim.Region.Physics.OdePlugin
528 d.Mass objMass; 546 d.Mass objMass;
529 d.BodyGetMass(Body, out objMass); 547 d.BodyGetMass(Body, out objMass);
530 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; 548 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
531 grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); 549 grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force
532 // Preserve the current Z velocity
533 d.Vector3 vel_now = d.BodyGetLinearVel(Body);
534 m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity
535 } // else its 1.0, no gravity. 550 } // else its 1.0, no gravity.
536 551
537 // Check if hovering 552 // Check if hovering
@@ -567,24 +582,24 @@ namespace OpenSim.Region.Physics.OdePlugin
567 { 582 {
568 d.Mass objMass; 583 d.Mass objMass;
569 d.BodyGetMass(Body, out objMass); 584 d.BodyGetMass(Body, out objMass);
570 m_dir.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale); 585 m_wLinObjectVel.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
571 //KF: m_VhoverEfficiency is not yet implemented 586 //KF: m_VhoverEfficiency is not yet implemented
572 } 587 }
573 else 588 else
574 { 589 {
575 m_dir.Z = 0f; 590 m_wLinObjectVel.Z = 0f;
576 } 591 }
577 } 592 }
578 593 else
594 { // not hovering, Gravity rules
595 m_wLinObjectVel.Z = vel_now.Z;
596//if(frcount == 0) Console.WriteLine(" Z {0} a.Z {1}", m_wLinObjectVel.Z, acceleration.Z);
597 }
579 // Apply velocity 598 // Apply velocity
580 d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z); 599 d.BodySetLinearVel(Body, m_wLinObjectVel.X, m_wLinObjectVel.Y, m_wLinObjectVel.Z);
581 // apply gravity force 600 // apply gravity force
582 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); 601 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
583 602//if(frcount == 0) Console.WriteLine("Grav {0}", grav);
584
585 // apply friction
586 Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
587 m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount * m_linearFrictionFactor;
588 } // end MoveLinear() 603 } // end MoveLinear()
589 604
590 private void MoveAngular(float pTimestep) 605 private void MoveAngular(float pTimestep)
@@ -633,7 +648,7 @@ namespace OpenSim.Region.Physics.OdePlugin
633 648
634 if(m_verticalAttractionTimescale < 300) 649 if(m_verticalAttractionTimescale < 300)
635 { 650 {
636 float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep); 651 float VAservo = 0.0167f / (m_verticalAttractionTimescale * pTimestep);
637 // get present body rotation 652 // get present body rotation
638 d.Quaternion rot = d.BodyGetQuaternion(Body); 653 d.Quaternion rot = d.BodyGetQuaternion(Body);
639 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); 654 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
generated by cgit v1.1 at 2024-12-14 22:12:53 +0000