diff options
Diffstat (limited to 'OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs')
-rw-r--r-- | OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs | 214 |
1 files changed, 142 insertions, 72 deletions
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs index 14d5caa..55d6945 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs | |||
@@ -108,13 +108,14 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
108 | 108 | ||
109 | //Angular properties | 109 | //Angular properties |
110 | 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 |
111 | private int m_angularMotorApply = 0; // application frame counter | 111 | |
112 | private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity | 112 | private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL |
113 | private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate | 113 | private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL |
114 | private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate | 114 | private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL |
115 | private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate | 115 | |
116 | private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor | ||
117 | // private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity | ||
116 | private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body | 118 | private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body |
117 | // private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body | ||
118 | 119 | ||
119 | //Deflection properties | 120 | //Deflection properties |
120 | // private float m_angularDeflectionEfficiency = 0; | 121 | // private float m_angularDeflectionEfficiency = 0; |
@@ -223,11 +224,13 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
223 | // These are vector properties but the engine lets you use a single float value to | 224 | // These are vector properties but the engine lets you use a single float value to |
224 | // set all of the components to the same value | 225 | // set all of the components to the same value |
225 | case Vehicle.ANGULAR_FRICTION_TIMESCALE: | 226 | case Vehicle.ANGULAR_FRICTION_TIMESCALE: |
227 | if (pValue > 30f) pValue = 30f; | ||
228 | if (pValue < 0.1f) pValue = 0.1f; | ||
226 | m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); | 229 | m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); |
227 | break; | 230 | break; |
228 | case Vehicle.ANGULAR_MOTOR_DIRECTION: | 231 | case Vehicle.ANGULAR_MOTOR_DIRECTION: |
229 | m_angularMotorDirection = new Vector3(pValue, pValue, pValue); | 232 | m_angularMotorDirection = new Vector3(pValue, pValue, pValue); |
230 | m_angularMotorApply = 10; | 233 | UpdateAngDecay(); |
231 | break; | 234 | break; |
232 | case Vehicle.LINEAR_FRICTION_TIMESCALE: | 235 | case Vehicle.LINEAR_FRICTION_TIMESCALE: |
233 | m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); | 236 | m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); |
@@ -249,6 +252,12 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
249 | switch (pParam) | 252 | switch (pParam) |
250 | { | 253 | { |
251 | case Vehicle.ANGULAR_FRICTION_TIMESCALE: | 254 | case Vehicle.ANGULAR_FRICTION_TIMESCALE: |
255 | if (pValue.X > 30f) pValue.X = 30f; | ||
256 | if (pValue.X < 0.1f) pValue.X = 0.1f; | ||
257 | if (pValue.Y > 30f) pValue.Y = 30f; | ||
258 | if (pValue.Y < 0.1f) pValue.Y = 0.1f; | ||
259 | if (pValue.Z > 30f) pValue.Z = 30f; | ||
260 | if (pValue.Z < 0.1f) pValue.Z = 0.1f; | ||
252 | m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); | 261 | m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); |
253 | break; | 262 | break; |
254 | case Vehicle.ANGULAR_MOTOR_DIRECTION: | 263 | case Vehicle.ANGULAR_MOTOR_DIRECTION: |
@@ -260,7 +269,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
260 | if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f; | 269 | if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f; |
261 | if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f; | 270 | if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f; |
262 | if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f; | 271 | if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f; |
263 | m_angularMotorApply = 10; | 272 | UpdateAngDecay(); |
264 | break; | 273 | break; |
265 | case Vehicle.LINEAR_FRICTION_TIMESCALE: | 274 | case Vehicle.LINEAR_FRICTION_TIMESCALE: |
266 | m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); | 275 | m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); |
@@ -305,11 +314,12 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
305 | { | 314 | { |
306 | case Vehicle.TYPE_SLED: | 315 | case Vehicle.TYPE_SLED: |
307 | m_linearFrictionTimescale = new Vector3(30, 1, 1000); | 316 | m_linearFrictionTimescale = new Vector3(30, 1, 1000); |
308 | m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); | 317 | m_angularFrictionTimescale = new Vector3(30, 30, 30); |
309 | // m_lLinMotorVel = Vector3.Zero; | 318 | // m_lLinMotorVel = Vector3.Zero; |
310 | m_linearMotorTimescale = 1000; | 319 | m_linearMotorTimescale = 1000; |
311 | m_linearMotorDecayTimescale = 120; | 320 | m_linearMotorDecayTimescale = 120; |
312 | m_angularMotorDirection = Vector3.Zero; | 321 | m_angularMotorDirection = Vector3.Zero; |
322 | m_angularMotorDVel = Vector3.Zero; | ||
313 | m_angularMotorTimescale = 1000; | 323 | m_angularMotorTimescale = 1000; |
314 | m_angularMotorDecayTimescale = 120; | 324 | m_angularMotorDecayTimescale = 120; |
315 | m_VhoverHeight = 0; | 325 | m_VhoverHeight = 0; |
@@ -331,11 +341,12 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
331 | break; | 341 | break; |
332 | case Vehicle.TYPE_CAR: | 342 | case Vehicle.TYPE_CAR: |
333 | m_linearFrictionTimescale = new Vector3(100, 2, 1000); | 343 | m_linearFrictionTimescale = new Vector3(100, 2, 1000); |
334 | m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); | 344 | m_angularFrictionTimescale = new Vector3(30, 30, 30); // was 1000, but sl max frict time is 30. |
335 | // m_lLinMotorVel = Vector3.Zero; | 345 | // m_lLinMotorVel = Vector3.Zero; |
336 | m_linearMotorTimescale = 1; | 346 | m_linearMotorTimescale = 1; |
337 | m_linearMotorDecayTimescale = 60; | 347 | m_linearMotorDecayTimescale = 60; |
338 | m_angularMotorDirection = Vector3.Zero; | 348 | m_angularMotorDirection = Vector3.Zero; |
349 | m_angularMotorDVel = Vector3.Zero; | ||
339 | m_angularMotorTimescale = 1; | 350 | m_angularMotorTimescale = 1; |
340 | m_angularMotorDecayTimescale = 0.8f; | 351 | m_angularMotorDecayTimescale = 0.8f; |
341 | m_VhoverHeight = 0; | 352 | m_VhoverHeight = 0; |
@@ -363,6 +374,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
363 | m_linearMotorTimescale = 5; | 374 | m_linearMotorTimescale = 5; |
364 | m_linearMotorDecayTimescale = 60; | 375 | m_linearMotorDecayTimescale = 60; |
365 | m_angularMotorDirection = Vector3.Zero; | 376 | m_angularMotorDirection = Vector3.Zero; |
377 | m_angularMotorDVel = Vector3.Zero; | ||
366 | m_angularMotorTimescale = 4; | 378 | m_angularMotorTimescale = 4; |
367 | m_angularMotorDecayTimescale = 4; | 379 | m_angularMotorDecayTimescale = 4; |
368 | m_VhoverHeight = 0; | 380 | m_VhoverHeight = 0; |
@@ -391,6 +403,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
391 | m_linearMotorTimescale = 2; | 403 | m_linearMotorTimescale = 2; |
392 | m_linearMotorDecayTimescale = 60; | 404 | m_linearMotorDecayTimescale = 60; |
393 | m_angularMotorDirection = Vector3.Zero; | 405 | m_angularMotorDirection = Vector3.Zero; |
406 | m_angularMotorDVel = Vector3.Zero; | ||
394 | m_angularMotorTimescale = 4; | 407 | m_angularMotorTimescale = 4; |
395 | m_angularMotorDecayTimescale = 4; | 408 | m_angularMotorDecayTimescale = 4; |
396 | m_VhoverHeight = 0; | 409 | m_VhoverHeight = 0; |
@@ -417,6 +430,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
417 | m_linearMotorTimescale = 5; | 430 | m_linearMotorTimescale = 5; |
418 | m_linearMotorDecayTimescale = 60; | 431 | m_linearMotorDecayTimescale = 60; |
419 | m_angularMotorDirection = Vector3.Zero; | 432 | m_angularMotorDirection = Vector3.Zero; |
433 | m_angularMotorDVel = Vector3.Zero; | ||
420 | m_angularMotorTimescale = 6; | 434 | m_angularMotorTimescale = 6; |
421 | m_angularMotorDecayTimescale = 10; | 435 | m_angularMotorDecayTimescale = 10; |
422 | m_VhoverHeight = 5; | 436 | m_VhoverHeight = 5; |
@@ -468,8 +482,8 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
468 | m_lLinObjectVel = Vector3.Zero; | 482 | m_lLinObjectVel = Vector3.Zero; |
469 | m_wLinObjectVel = Vector3.Zero; | 483 | m_wLinObjectVel = Vector3.Zero; |
470 | m_angularMotorDirection = Vector3.Zero; | 484 | m_angularMotorDirection = Vector3.Zero; |
471 | m_angularMotorVelocity = Vector3.Zero; | 485 | m_lastAngularVelocity = Vector3.Zero; |
472 | m_lastAngularVelocity = Vector3.Zero; | 486 | m_angularMotorDVel = Vector3.Zero; |
473 | } | 487 | } |
474 | 488 | ||
475 | private void UpdateLinDecay() | 489 | private void UpdateLinDecay() |
@@ -542,6 +556,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
542 | if (m_linearFrictionTimescale.Z < 300.0f) | 556 | if (m_linearFrictionTimescale.Z < 300.0f) |
543 | { | 557 | { |
544 | float fricfactor = m_linearFrictionTimescale.Z / pTimestep; | 558 | float fricfactor = m_linearFrictionTimescale.Z / pTimestep; |
559 | //if(frcount == 0) Console.WriteLine("Zfric={0}", fricfactor); | ||
545 | float fricZ = m_lLinObjectVel.Z / fricfactor; | 560 | float fricZ = m_lLinObjectVel.Z / fricfactor; |
546 | m_lLinObjectVel.Z -= fricZ; | 561 | m_lLinObjectVel.Z -= fricZ; |
547 | } | 562 | } |
@@ -611,54 +626,73 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
611 | d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); | 626 | d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); |
612 | //if(frcount == 0) Console.WriteLine("Grav {0}", grav); | 627 | //if(frcount == 0) Console.WriteLine("Grav {0}", grav); |
613 | } // end MoveLinear() | 628 | } // end MoveLinear() |
629 | |||
630 | private void UpdateAngDecay() | ||
631 | { | ||
632 | if (Math.Abs(m_angularMotorDirection.X) > Math.Abs(m_angularMotorDVel.X)) m_angularMotorDVel.X = m_angularMotorDirection.X; | ||
633 | if (Math.Abs(m_angularMotorDirection.Y) > Math.Abs(m_angularMotorDVel.Y)) m_angularMotorDVel.Y = m_angularMotorDirection.Y; | ||
634 | if (Math.Abs(m_angularMotorDirection.Z) > Math.Abs(m_angularMotorDVel.Z)) m_angularMotorDVel.Z = m_angularMotorDirection.Z; | ||
635 | } // else let the motor decay on its own | ||
614 | 636 | ||
615 | private void MoveAngular(float pTimestep) | 637 | private void MoveAngular(float pTimestep) |
616 | { | 638 | { |
617 | /* | 639 | /* |
618 | private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor | 640 | private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor |
619 | private int m_angularMotorApply = 0; // application frame counter | 641 | |
620 | private float m_angularMotorVelocity = 0; // current angular motor velocity (ramps up and down) | 642 | private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL |
621 | private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate | 643 | private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL |
622 | private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate | 644 | private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL |
623 | private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate | 645 | |
624 | private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body | 646 | private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor |
647 | private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body | ||
625 | */ | 648 | */ |
626 | //if(frcount == 0) Console.WriteLine("MoveAngular "); | 649 | //if(frcount == 0) Console.WriteLine("MoveAngular "); |
627 | 650 | ||
628 | // Get what the body is doing, this includes 'external' influences | 651 | // Get what the body is doing, this includes 'external' influences |
629 | d.Vector3 angularVelocity = d.BodyGetAngularVel(Body); | 652 | d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body); |
630 | // Vector3 angularVelocity = Vector3.Zero; | 653 | Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z); |
654 | //if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel); | ||
655 | // Vector3 FrAaccel = m_lastAngularVelocity - angObjectVel; | ||
656 | // Vector3 initavel = angObjectVel; | ||
657 | // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack. | ||
658 | float atk_decayfactor = 23.0f / (m_angularMotorTimescale * pTimestep); | ||
659 | m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor; | ||
660 | // Decay Angular Motor 2. | ||
661 | if (m_angularMotorDecayTimescale < 300.0f) | ||
662 | { | ||
663 | //#### | ||
664 | if ( Vector3.Mag(m_angularMotorDVel) < 1.0f) | ||
665 | { | ||
666 | float decayfactor = (m_angularMotorDecayTimescale)/pTimestep; | ||
667 | Vector3 decayAmount = (m_angularMotorDVel/decayfactor); | ||
668 | m_angularMotorDVel -= decayAmount; | ||
669 | } | ||
670 | else | ||
671 | { | ||
672 | Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * pTimestep / m_angularMotorDecayTimescale; | ||
673 | m_angularMotorDVel -= decel; | ||
674 | } | ||
631 | 675 | ||
632 | if (m_angularMotorApply > 0) | 676 | if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) |
633 | { | 677 | { |
634 | // ramp up to new value | 678 | m_angularMotorDVel = Vector3.Zero; |
635 | // current velocity += error / ( time to get there / step interval ) | 679 | } |
636 | // requested speed - last motor speed | 680 | else |
637 | m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep); | 681 | { |
638 | m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep); | 682 | if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X; |
639 | m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep); | 683 | if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y; |
640 | 684 | if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z; | |
641 | m_angularMotorApply--; // This is done so that if script request rate is less than phys frame rate the expected | 685 | } |
642 | // velocity may still be acheived. | 686 | } // end decay angular motor |
643 | } | 687 | //if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel); |
644 | else | ||
645 | { | ||
646 | // no motor recently applied, keep the body velocity | ||
647 | /* m_angularMotorVelocity.X = angularVelocity.X; | ||
648 | m_angularMotorVelocity.Y = angularVelocity.Y; | ||
649 | m_angularMotorVelocity.Z = angularVelocity.Z; */ | ||
650 | |||
651 | // and decay the velocity | ||
652 | m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep); | ||
653 | } // end motor section | ||
654 | |||
655 | 688 | ||
689 | //if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel); | ||
656 | // Vertical attractor section | 690 | // Vertical attractor section |
657 | Vector3 vertattr = Vector3.Zero; | 691 | Vector3 vertattr = Vector3.Zero; |
658 | 692 | ||
659 | if(m_verticalAttractionTimescale < 300) | 693 | if(m_verticalAttractionTimescale < 300) |
660 | { | 694 | { |
661 | float VAservo = 0.0167f / (m_verticalAttractionTimescale * pTimestep); | 695 | float VAservo = 1.0f / (m_verticalAttractionTimescale * pTimestep); |
662 | // get present body rotation | 696 | // get present body rotation |
663 | d.Quaternion rot = d.BodyGetQuaternion(Body); | 697 | d.Quaternion rot = d.BodyGetQuaternion(Body); |
664 | Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); | 698 | Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); |
@@ -670,38 +704,75 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
670 | // 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. | 704 | // 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. |
671 | // 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 | 705 | // 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 |
672 | // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. | 706 | // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. |
707 | |||
673 | if (verterr.Z < 0.0f) | 708 | if (verterr.Z < 0.0f) |
674 | { | 709 | { // Defelction from vertical exceeds 90-degrees. This method will ensure stable return to |
710 | // vertical, BUT for some reason a z-rotation is imparted to the object. TBI. | ||
711 | //Console.WriteLine("InvertFlip"); | ||
675 | verterr.X = 2.0f - verterr.X; | 712 | verterr.X = 2.0f - verterr.X; |
676 | verterr.Y = 2.0f - verterr.Y; | 713 | verterr.Y = 2.0f - verterr.Y; |
677 | } | 714 | } |
678 | // Error is 0 (no error) to +/- 2 (max error) | 715 | verterr *= 0.5f; |
679 | // scale it by VAservo | 716 | // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt) |
680 | verterr = verterr * VAservo; | 717 | |
718 | if ((!angObjectVel.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f)) | ||
719 | { | ||
720 | //if(frcount == 0) | ||
721 | // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so | ||
722 | // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. | ||
723 | vertattr.X = verterr.Y; | ||
724 | vertattr.Y = - verterr.X; | ||
725 | vertattr.Z = 0f; | ||
681 | //if(frcount == 0) Console.WriteLine("VAerr=" + verterr); | 726 | //if(frcount == 0) Console.WriteLine("VAerr=" + verterr); |
682 | 727 | ||
683 | // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so | 728 | // scaling appears better usingsquare-law |
684 | // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. | 729 | float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency; |
685 | vertattr.X = verterr.Y; | 730 | float bounce = 1.0f - damped; |
686 | vertattr.Y = - verterr.X; | 731 | // 0 = crit damp, 1 = bouncy |
687 | vertattr.Z = 0f; | 732 | float oavz = angObjectVel.Z; // retain z velocity |
688 | 733 | angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce; // The time-scaled correction, which sums, therefore is bouncy | |
689 | // scaling appears better usingsquare-law | 734 | angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped); // damped, good @ < 90. |
690 | float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency); | 735 | angObjectVel.Z = oavz; |
691 | vertattr.X += bounce * angularVelocity.X; | 736 | //if(frcount == 0) Console.WriteLine("VA+"); |
692 | vertattr.Y += bounce * angularVelocity.Y; | 737 | //Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel); |
693 | 738 | } | |
739 | else | ||
740 | { | ||
741 | // else error is very small | ||
742 | angObjectVel.X = 0f; | ||
743 | angObjectVel.Y = 0f; | ||
744 | //if(frcount == 0) Console.WriteLine("VA0"); | ||
745 | } | ||
694 | } // else vertical attractor is off | 746 | } // else vertical attractor is off |
747 | //if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel); | ||
748 | |||
749 | if ( (! m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) ) | ||
750 | { // if motor or object have motion | ||
751 | if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); | ||
752 | |||
753 | if (m_angularMotorTimescale < 300.0f) | ||
754 | { | ||
755 | Vector3 attack_error = m_angularMotorDVel - angObjectVel; | ||
756 | float angfactor = m_angularMotorTimescale/pTimestep; | ||
757 | Vector3 attackAmount = (attack_error/angfactor); | ||
758 | angObjectVel += attackAmount; | ||
759 | //if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount); | ||
760 | //if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel); | ||
761 | } | ||
762 | |||
763 | angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / pTimestep); | ||
764 | angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / pTimestep); | ||
765 | angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / pTimestep); | ||
766 | } // else no signif. motion | ||
695 | 767 | ||
696 | // m_lastVertAttractor = vertattr; | 768 | //if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel); |
697 | |||
698 | // Bank section tba | 769 | // Bank section tba |
699 | // Deflection section tba | 770 | // Deflection section tba |
771 | //if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel); | ||
700 | 772 | ||
701 | // Sum velocities | 773 | m_lastAngularVelocity = angObjectVel; |
702 | m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // tba: + bank + deflection | 774 | /* |
703 | 775 | if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.0001f)) | |
704 | if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f)) | ||
705 | { | 776 | { |
706 | if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); | 777 | if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); |
707 | } | 778 | } |
@@ -709,13 +780,12 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
709 | { | 780 | { |
710 | m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero. | 781 | m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero. |
711 | } | 782 | } |
712 | 783 | */ | |
713 | // apply friction | ||
714 | Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep); | ||
715 | m_lastAngularVelocity -= m_lastAngularVelocity * decayamount; | ||
716 | |||
717 | // Apply to the body | 784 | // Apply to the body |
785 | // Vector3 aInc = m_lastAngularVelocity - initavel; | ||
786 | //if(frcount == 0) Console.WriteLine("Inc {0}", aInc); | ||
718 | d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); | 787 | d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); |
788 | //if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity); | ||
719 | 789 | ||
720 | } //end MoveAngular | 790 | } //end MoveAngular |
721 | } | 791 | } |