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.cs197
1 files changed, 111 insertions, 86 deletions
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
index ef2dccc..14d5caa 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);
@@ -294,7 +286,17 @@ namespace OpenSim.Region.Physics.OdePlugin
294 } 286 }
295 287
296 }//end ProcessRotationVehicleParam 288 }//end ProcessRotationVehicleParam
289
290 internal void ProcessFlagsVehicleSet(int flags)
291 {
292 m_flags |= (VehicleFlag)flags;
293 }
297 294
295 internal void ProcessFlagsVehicleRemove(int flags)
296 {
297 m_flags &= ~((VehicleFlag)flags);
298 }
299
298 internal void ProcessTypeChange(Vehicle pType) 300 internal void ProcessTypeChange(Vehicle pType)
299 { 301 {
300 // Set Defaults For Type 302 // Set Defaults For Type
@@ -304,7 +306,7 @@ namespace OpenSim.Region.Physics.OdePlugin
304 case Vehicle.TYPE_SLED: 306 case Vehicle.TYPE_SLED:
305 m_linearFrictionTimescale = new Vector3(30, 1, 1000); 307 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
306 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); 308 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
307 m_linearMotorDirection = Vector3.Zero; 309// m_lLinMotorVel = Vector3.Zero;
308 m_linearMotorTimescale = 1000; 310 m_linearMotorTimescale = 1000;
309 m_linearMotorDecayTimescale = 120; 311 m_linearMotorDecayTimescale = 120;
310 m_angularMotorDirection = Vector3.Zero; 312 m_angularMotorDirection = Vector3.Zero;
@@ -330,7 +332,7 @@ namespace OpenSim.Region.Physics.OdePlugin
330 case Vehicle.TYPE_CAR: 332 case Vehicle.TYPE_CAR:
331 m_linearFrictionTimescale = new Vector3(100, 2, 1000); 333 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
332 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); 334 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
333 m_linearMotorDirection = Vector3.Zero; 335// m_lLinMotorVel = Vector3.Zero;
334 m_linearMotorTimescale = 1; 336 m_linearMotorTimescale = 1;
335 m_linearMotorDecayTimescale = 60; 337 m_linearMotorDecayTimescale = 60;
336 m_angularMotorDirection = Vector3.Zero; 338 m_angularMotorDirection = Vector3.Zero;
@@ -357,7 +359,7 @@ namespace OpenSim.Region.Physics.OdePlugin
357 case Vehicle.TYPE_BOAT: 359 case Vehicle.TYPE_BOAT:
358 m_linearFrictionTimescale = new Vector3(10, 3, 2); 360 m_linearFrictionTimescale = new Vector3(10, 3, 2);
359 m_angularFrictionTimescale = new Vector3(10,10,10); 361 m_angularFrictionTimescale = new Vector3(10,10,10);
360 m_linearMotorDirection = Vector3.Zero; 362// m_lLinMotorVel = Vector3.Zero;
361 m_linearMotorTimescale = 5; 363 m_linearMotorTimescale = 5;
362 m_linearMotorDecayTimescale = 60; 364 m_linearMotorDecayTimescale = 60;
363 m_angularMotorDirection = Vector3.Zero; 365 m_angularMotorDirection = Vector3.Zero;
@@ -385,7 +387,7 @@ namespace OpenSim.Region.Physics.OdePlugin
385 case Vehicle.TYPE_AIRPLANE: 387 case Vehicle.TYPE_AIRPLANE:
386 m_linearFrictionTimescale = new Vector3(200, 10, 5); 388 m_linearFrictionTimescale = new Vector3(200, 10, 5);
387 m_angularFrictionTimescale = new Vector3(20, 20, 20); 389 m_angularFrictionTimescale = new Vector3(20, 20, 20);
388 m_linearMotorDirection = Vector3.Zero; 390// m_lLinMotorVel = Vector3.Zero;
389 m_linearMotorTimescale = 2; 391 m_linearMotorTimescale = 2;
390 m_linearMotorDecayTimescale = 60; 392 m_linearMotorDecayTimescale = 60;
391 m_angularMotorDirection = Vector3.Zero; 393 m_angularMotorDirection = Vector3.Zero;
@@ -412,7 +414,6 @@ namespace OpenSim.Region.Physics.OdePlugin
412 case Vehicle.TYPE_BALLOON: 414 case Vehicle.TYPE_BALLOON:
413 m_linearFrictionTimescale = new Vector3(5, 5, 5); 415 m_linearFrictionTimescale = new Vector3(5, 5, 5);
414 m_angularFrictionTimescale = new Vector3(10, 10, 10); 416 m_angularFrictionTimescale = new Vector3(10, 10, 10);
415 m_linearMotorDirection = Vector3.Zero;
416 m_linearMotorTimescale = 5; 417 m_linearMotorTimescale = 5;
417 m_linearMotorDecayTimescale = 60; 418 m_linearMotorDecayTimescale = 60;
418 m_angularMotorDirection = Vector3.Zero; 419 m_angularMotorDirection = Vector3.Zero;
@@ -453,7 +454,7 @@ namespace OpenSim.Region.Physics.OdePlugin
453 if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE) 454 if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
454 return; 455 return;
455 frcount++; // used to limit debug comment output 456 frcount++; // used to limit debug comment output
456 if (frcount > 100) 457 if (frcount > 24)
457 frcount = 0; 458 frcount = 0;
458 459
459 MoveLinear(pTimestep, pParentScene); 460 MoveLinear(pTimestep, pParentScene);
@@ -463,63 +464,90 @@ namespace OpenSim.Region.Physics.OdePlugin
463 internal void Halt() 464 internal void Halt()
464 { // Kill all motions, when non-physical 465 { // Kill all motions, when non-physical
465 m_linearMotorDirection = Vector3.Zero; 466 m_linearMotorDirection = Vector3.Zero;
466 m_linearMotorDirectionLASTSET = Vector3.Zero; 467 m_lLinMotorDVel = Vector3.Zero;
467 m_dir = Vector3.Zero; 468 m_lLinObjectVel = Vector3.Zero;
468 m_lastLinearVelocityVector = Vector3.Zero; 469 m_wLinObjectVel = Vector3.Zero;
469 m_angularMotorDirection = Vector3.Zero; 470 m_angularMotorDirection = Vector3.Zero;
470 m_angularMotorVelocity = Vector3.Zero; 471 m_angularMotorVelocity = Vector3.Zero;
471 m_lastAngularVelocity = Vector3.Zero; 472 m_lastAngularVelocity = Vector3.Zero;
472 } 473 }
474
475 private void UpdateLinDecay()
476 {
477 if (Math.Abs(m_linearMotorDirection.X) > Math.Abs(m_lLinMotorDVel.X)) m_lLinMotorDVel.X = m_linearMotorDirection.X;
478 if (Math.Abs(m_linearMotorDirection.Y) > Math.Abs(m_lLinMotorDVel.Y)) m_lLinMotorDVel.Y = m_linearMotorDirection.Y;
479 if (Math.Abs(m_linearMotorDirection.Z) > Math.Abs(m_lLinMotorDVel.Z)) m_lLinMotorDVel.Z = m_linearMotorDirection.Z;
480 } // else let the motor decay on its own
473 481
474 private void MoveLinear(float pTimestep, OdeScene _pParentScene) 482 private void MoveLinear(float pTimestep, OdeScene _pParentScene)
475 { 483 {
476 if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant 484 Vector3 acceleration = new Vector3(0f, 0f, 0f);
477 {
478 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
479 485
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); 486 d.Quaternion rot = d.BodyGetQuaternion(Body);
516 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object 487 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 488 Quaternion irotq = Quaternion.Inverse(rotq);
489 d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame
490 Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z);
491 acceleration = vel_now - m_wLinObjectVel;
492 m_lLinObjectVel = vel_now * irotq;
493
494 if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate
495 {
496 if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f)
497 {
498 float decayfactor = m_linearMotorDecayTimescale/pTimestep;
499 Vector3 decayAmount = (m_lLinMotorDVel/decayfactor);
500 m_lLinMotorDVel -= decayAmount;
501 }
502 else
503 {
504 float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale)));
505 Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * pTimestep;
506 m_lLinMotorDVel -= decel;
507 }
508 if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
509 {
510 m_lLinMotorDVel = Vector3.Zero;
511 }
512 else
513 {
514 if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X;
515 if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y;
516 if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z;
517 }
518 }
518 519
519 // add Gravity and Buoyancy 520 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 521 {
521 // .Z velocity and gravity. Therefore only 0g will used script-requested 522 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
522 // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only. 523 if (m_linearMotorTimescale < 300.0f)
524 {
525 Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel;
526 float linfactor = m_linearMotorTimescale/pTimestep;
527 Vector3 attackAmount = (attack_error/linfactor) * 1.3f;
528 m_lLinObjectVel += attackAmount;
529 }
530 if (m_linearFrictionTimescale.X < 300.0f)
531 {
532 float fricfactor = m_linearFrictionTimescale.X / pTimestep;
533 float fricX = m_lLinObjectVel.X / fricfactor;
534 m_lLinObjectVel.X -= fricX;
535 }
536 if (m_linearFrictionTimescale.Y < 300.0f)
537 {
538 float fricfactor = m_linearFrictionTimescale.Y / pTimestep;
539 float fricY = m_lLinObjectVel.Y / fricfactor;
540 m_lLinObjectVel.Y -= fricY;
541 }
542 if (m_linearFrictionTimescale.Z < 300.0f)
543 {
544 float fricfactor = m_linearFrictionTimescale.Z / pTimestep;
545 float fricZ = m_lLinObjectVel.Z / fricfactor;
546 m_lLinObjectVel.Z -= fricZ;
547 }
548 }
549 m_wLinObjectVel = m_lLinObjectVel * rotq;
550 // Add Gravity and Buoyancy
523 Vector3 grav = Vector3.Zero; 551 Vector3 grav = Vector3.Zero;
524 if(m_VehicleBuoyancy < 1.0f) 552 if(m_VehicleBuoyancy < 1.0f)
525 { 553 {
@@ -528,10 +556,7 @@ namespace OpenSim.Region.Physics.OdePlugin
528 d.Mass objMass; 556 d.Mass objMass;
529 d.BodyGetMass(Body, out objMass); 557 d.BodyGetMass(Body, out objMass);
530 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; 558 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
531 grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); 559 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. 560 } // else its 1.0, no gravity.
536 561
537 // Check if hovering 562 // Check if hovering
@@ -567,24 +592,24 @@ namespace OpenSim.Region.Physics.OdePlugin
567 { 592 {
568 d.Mass objMass; 593 d.Mass objMass;
569 d.BodyGetMass(Body, out objMass); 594 d.BodyGetMass(Body, out objMass);
570 m_dir.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale); 595 m_wLinObjectVel.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
571 //KF: m_VhoverEfficiency is not yet implemented 596 //KF: m_VhoverEfficiency is not yet implemented
572 } 597 }
573 else 598 else
574 { 599 {
575 m_dir.Z = 0f; 600 m_wLinObjectVel.Z = 0f;
576 } 601 }
577 } 602 }
578 603 else
604 { // not hovering, Gravity rules
605 m_wLinObjectVel.Z = vel_now.Z;
606//if(frcount == 0) Console.WriteLine(" Z {0} a.Z {1}", m_wLinObjectVel.Z, acceleration.Z);
607 }
579 // Apply velocity 608 // Apply velocity
580 d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z); 609 d.BodySetLinearVel(Body, m_wLinObjectVel.X, m_wLinObjectVel.Y, m_wLinObjectVel.Z);
581 // apply gravity force 610 // apply gravity force
582 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); 611 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
583 612//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() 613 } // end MoveLinear()
589 614
590 private void MoveAngular(float pTimestep) 615 private void MoveAngular(float pTimestep)
@@ -633,7 +658,7 @@ namespace OpenSim.Region.Physics.OdePlugin
633 658
634 if(m_verticalAttractionTimescale < 300) 659 if(m_verticalAttractionTimescale < 300)
635 { 660 {
636 float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep); 661 float VAservo = 0.0167f / (m_verticalAttractionTimescale * pTimestep);
637 // get present body rotation 662 // get present body rotation
638 d.Quaternion rot = d.BodyGetQuaternion(Body); 663 d.Quaternion rot = d.BodyGetQuaternion(Body);
639 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); 664 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
generated by cgit v1.1 at 2024-12-18 04:09:58 +0000