aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
diff options
context:
space:
mode:
authorBlueWall2012-11-26 06:55:03 -0500
committerBlueWall2012-11-26 06:55:03 -0500
commit88f2fbc8f165121fc63328aec60963a25d07082f (patch)
tree459d9986d18f002415c4694a6db7c3ac1efb237e /OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
parentMerge branch 'master' into connector_plugin (diff)
parentBulletSim: use m_angularMotor to do the basic movement. Add the setting of sa... (diff)
downloadopensim-SC_OLD-88f2fbc8f165121fc63328aec60963a25d07082f.zip
opensim-SC_OLD-88f2fbc8f165121fc63328aec60963a25d07082f.tar.gz
opensim-SC_OLD-88f2fbc8f165121fc63328aec60963a25d07082f.tar.bz2
opensim-SC_OLD-88f2fbc8f165121fc63328aec60963a25d07082f.tar.xz
Merge branch 'master' into connector_plugin
Diffstat (limited to 'OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs')
-rw-r--r--OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs307
1 files changed, 149 insertions, 158 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index dbc9039..95a4134 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -80,10 +80,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
80 private Quaternion m_referenceFrame = Quaternion.Identity; 80 private Quaternion m_referenceFrame = Quaternion.Identity;
81 81
82 // Linear properties 82 // Linear properties
83 private BSVMotor m_linearMotor = new BSVMotor("LinearMotor");
83 private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time 84 private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time
84 private Vector3 m_linearMotorOffset = Vector3.Zero; // the point of force can be offset from the center 85 private Vector3 m_linearMotorOffset = Vector3.Zero; // the point of force can be offset from the center
85 private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL 86 private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL
86 private Vector3 m_newVelocity = Vector3.Zero; // velocity computed to be applied to body
87 private Vector3 m_linearFrictionTimescale = Vector3.Zero; 87 private Vector3 m_linearFrictionTimescale = Vector3.Zero;
88 private float m_linearMotorDecayTimescale = 0; 88 private float m_linearMotorDecayTimescale = 0;
89 private float m_linearMotorTimescale = 0; 89 private float m_linearMotorTimescale = 0;
@@ -93,6 +93,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
93 // private Vector3 m_linearMotorOffset = Vector3.Zero; 93 // private Vector3 m_linearMotorOffset = Vector3.Zero;
94 94
95 //Angular properties 95 //Angular properties
96 private BSVMotor m_angularMotor = new BSVMotor("AngularMotor");
96 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor 97 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
97 // private int m_angularMotorApply = 0; // application frame counter 98 // private int m_angularMotorApply = 0; // application frame counter
98 private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity 99 private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity
@@ -152,10 +153,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
152 m_angularDeflectionTimescale = Math.Max(pValue, 0.01f); 153 m_angularDeflectionTimescale = Math.Max(pValue, 0.01f);
153 break; 154 break;
154 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: 155 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
155 m_angularMotorDecayTimescale = Math.Max(pValue, 0.01f); 156 m_angularMotorDecayTimescale = Math.Max(0.01f, Math.Min(pValue,120));
157 m_angularMotor.TargetValueDecayTimeScale = m_angularMotorDecayTimescale;
156 break; 158 break;
157 case Vehicle.ANGULAR_MOTOR_TIMESCALE: 159 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
158 m_angularMotorTimescale = Math.Max(pValue, 0.01f); 160 m_angularMotorTimescale = Math.Max(pValue, 0.01f);
161 m_angularMotor.TimeScale = m_angularMotorTimescale;
159 break; 162 break;
160 case Vehicle.BANKING_EFFICIENCY: 163 case Vehicle.BANKING_EFFICIENCY:
161 m_bankingEfficiency = Math.Max(-1f, Math.Min(pValue, 1f)); 164 m_bankingEfficiency = Math.Max(-1f, Math.Min(pValue, 1f));
@@ -185,10 +188,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
185 m_linearDeflectionTimescale = Math.Max(pValue, 0.01f); 188 m_linearDeflectionTimescale = Math.Max(pValue, 0.01f);
186 break; 189 break;
187 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: 190 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
188 m_linearMotorDecayTimescale = Math.Max(pValue, 0.01f); 191 m_linearMotorDecayTimescale = Math.Max(0.01f, Math.Min(pValue,120));
192 m_linearMotor.TargetValueDecayTimeScale = m_linearMotorDecayTimescale;
189 break; 193 break;
190 case Vehicle.LINEAR_MOTOR_TIMESCALE: 194 case Vehicle.LINEAR_MOTOR_TIMESCALE:
191 m_linearMotorTimescale = Math.Max(pValue, 0.01f); 195 m_linearMotorTimescale = Math.Max(pValue, 0.01f);
196 m_linearMotor.TimeScale = m_linearMotorTimescale;
192 break; 197 break;
193 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: 198 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
194 m_verticalAttractionEfficiency = Math.Max(0.1f, Math.Min(pValue, 1f)); 199 m_verticalAttractionEfficiency = Math.Max(0.1f, Math.Min(pValue, 1f));
@@ -201,17 +206,20 @@ namespace OpenSim.Region.Physics.BulletSPlugin
201 // set all of the components to the same value 206 // set all of the components to the same value
202 case Vehicle.ANGULAR_FRICTION_TIMESCALE: 207 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
203 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); 208 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
209 m_angularMotor.FrictionTimescale = m_angularFrictionTimescale;
204 break; 210 break;
205 case Vehicle.ANGULAR_MOTOR_DIRECTION: 211 case Vehicle.ANGULAR_MOTOR_DIRECTION:
206 m_angularMotorDirection = new Vector3(pValue, pValue, pValue); 212 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
207 // m_angularMotorApply = 100; 213 m_angularMotor.SetTarget(m_angularMotorDirection);
208 break; 214 break;
209 case Vehicle.LINEAR_FRICTION_TIMESCALE: 215 case Vehicle.LINEAR_FRICTION_TIMESCALE:
210 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); 216 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
217 m_linearMotor.FrictionTimescale = m_linearFrictionTimescale;
211 break; 218 break;
212 case Vehicle.LINEAR_MOTOR_DIRECTION: 219 case Vehicle.LINEAR_MOTOR_DIRECTION:
213 m_linearMotorDirection = new Vector3(pValue, pValue, pValue); 220 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
214 m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue); 221 m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue);
222 m_linearMotor.SetTarget(m_linearMotorDirection);
215 break; 223 break;
216 case Vehicle.LINEAR_MOTOR_OFFSET: 224 case Vehicle.LINEAR_MOTOR_OFFSET:
217 m_linearMotorOffset = new Vector3(pValue, pValue, pValue); 225 m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
@@ -227,6 +235,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
227 { 235 {
228 case Vehicle.ANGULAR_FRICTION_TIMESCALE: 236 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
229 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); 237 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
238 m_angularMotor.FrictionTimescale = m_angularFrictionTimescale;
230 break; 239 break;
231 case Vehicle.ANGULAR_MOTOR_DIRECTION: 240 case Vehicle.ANGULAR_MOTOR_DIRECTION:
232 // Limit requested angular speed to 2 rps= 4 pi rads/sec 241 // Limit requested angular speed to 2 rps= 4 pi rads/sec
@@ -234,14 +243,16 @@ namespace OpenSim.Region.Physics.BulletSPlugin
234 pValue.Y = Math.Max(-12.56f, Math.Min(pValue.Y, 12.56f)); 243 pValue.Y = Math.Max(-12.56f, Math.Min(pValue.Y, 12.56f));
235 pValue.Z = Math.Max(-12.56f, Math.Min(pValue.Z, 12.56f)); 244 pValue.Z = Math.Max(-12.56f, Math.Min(pValue.Z, 12.56f));
236 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); 245 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
237 // m_angularMotorApply = 100; 246 m_angularMotor.SetTarget(m_angularMotorDirection);
238 break; 247 break;
239 case Vehicle.LINEAR_FRICTION_TIMESCALE: 248 case Vehicle.LINEAR_FRICTION_TIMESCALE:
240 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); 249 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
250 m_linearMotor.FrictionTimescale = m_linearFrictionTimescale;
241 break; 251 break;
242 case Vehicle.LINEAR_MOTOR_DIRECTION: 252 case Vehicle.LINEAR_MOTOR_DIRECTION:
243 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); 253 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
244 m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); 254 m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
255 m_linearMotor.SetTarget(m_linearMotorDirection);
245 break; 256 break;
246 case Vehicle.LINEAR_MOTOR_OFFSET: 257 case Vehicle.LINEAR_MOTOR_OFFSET:
247 m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); 258 m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
@@ -319,6 +330,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
319 330
320 m_referenceFrame = Quaternion.Identity; 331 m_referenceFrame = Quaternion.Identity;
321 m_flags = (VehicleFlag)0; 332 m_flags = (VehicleFlag)0;
333
322 break; 334 break;
323 335
324 case Vehicle.TYPE_SLED: 336 case Vehicle.TYPE_SLED:
@@ -351,10 +363,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
351 m_bankingMix = 1; 363 m_bankingMix = 1;
352 364
353 m_referenceFrame = Quaternion.Identity; 365 m_referenceFrame = Quaternion.Identity;
354 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP); 366 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
355 m_flags &= 367 | VehicleFlag.HOVER_TERRAIN_ONLY
356 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | 368 | VehicleFlag.HOVER_GLOBAL_HEIGHT
357 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); 369 | VehicleFlag.HOVER_UP_ONLY);
370 m_flags |= (VehicleFlag.NO_DEFLECTION_UP
371 | VehicleFlag.LIMIT_ROLL_ONLY
372 | VehicleFlag.LIMIT_MOTOR_UP);
358 break; 373 break;
359 case Vehicle.TYPE_CAR: 374 case Vehicle.TYPE_CAR:
360 m_linearMotorDirection = Vector3.Zero; 375 m_linearMotorDirection = Vector3.Zero;
@@ -510,6 +525,18 @@ namespace OpenSim.Region.Physics.BulletSPlugin
510 | VehicleFlag.HOVER_GLOBAL_HEIGHT); 525 | VehicleFlag.HOVER_GLOBAL_HEIGHT);
511 break; 526 break;
512 } 527 }
528
529 // Update any physical parameters based on this type.
530 Refresh();
531
532 m_linearMotor = new BSVMotor("LinearMotor", m_linearMotorTimescale,
533 m_linearMotorDecayTimescale, m_linearFrictionTimescale, 1f);
534 m_linearMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG DEBUG (enables detail logging)
535 m_angularMotor = new BSVMotor("AngularMotor", m_angularMotorTimescale,
536 m_angularMotorDecayTimescale, m_angularFrictionTimescale, 1f);
537 m_angularMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG DEBUG (enables detail logging)
538
539 // m_bankingMotor = new BSVMotor("BankingMotor", ...);
513 } 540 }
514 541
515 // Some of the properties of this prim may have changed. 542 // Some of the properties of this prim may have changed.
@@ -518,13 +545,25 @@ namespace OpenSim.Region.Physics.BulletSPlugin
518 { 545 {
519 if (IsActive) 546 if (IsActive)
520 { 547 {
521 // Friction effects are handled by this vehicle code 548 m_vehicleMass = Prim.Linkset.LinksetMass;
522 BulletSimAPI.SetFriction2(Prim.PhysBody.ptr, 0f);
523 BulletSimAPI.SetHitFraction2(Prim.PhysBody.ptr, 0f);
524
525 // BulletSimAPI.SetAngularDamping2(Prim.PhysBody.ptr, 0.8f);
526 549
527 VDetailLog("{0},BSDynamics.Refresh,zeroingFriction and adding damping", Prim.LocalID); 550 // Friction effects are handled by this vehicle code
551 float friction = 0f;
552 BulletSimAPI.SetFriction2(Prim.PhysBody.ptr, friction);
553
554 // Moderate angular movement introduced by Bullet.
555 // TODO: possibly set AngularFactor and LinearFactor for the type of vehicle.
556 // Maybe compute linear and angular factor and damping from params.
557 float angularDamping = PhysicsScene.Params.vehicleAngularDamping;
558 BulletSimAPI.SetAngularDamping2(Prim.PhysBody.ptr, angularDamping);
559
560 // DEBUG DEBUG DEBUG: use uniform inertia to smooth movement added by Bullet
561 // Vector3 localInertia = new Vector3(1f, 1f, 1f);
562 Vector3 localInertia = new Vector3(m_vehicleMass, m_vehicleMass, m_vehicleMass);
563 BulletSimAPI.SetMassProps2(Prim.PhysBody.ptr, m_vehicleMass, localInertia);
564
565 VDetailLog("{0},BSDynamics.Refresh,frict={1},inert={2},aDamp={3}",
566 Prim.LocalID, friction, localInertia, angularDamping);
528 } 567 }
529 } 568 }
530 569
@@ -551,97 +590,38 @@ namespace OpenSim.Region.Physics.BulletSPlugin
551 { 590 {
552 if (!IsActive) return; 591 if (!IsActive) return;
553 592
554 // DEBUG
555 // Because Bullet does apply forces to the vehicle, our last computed
556 // linear and angular velocities are not what is happening now.
557 // Vector3 externalAngularVelocity = Prim.ForceRotationalVelocity - m_lastAngularVelocity;
558 // m_lastAngularVelocity += (externalAngularVelocity * 0.5f) * pTimestep;
559 // m_lastAngularVelocity = Prim.ForceRotationalVelocity; // DEBUG: account for what Bullet did last time
560 // m_lastLinearVelocityVector = Prim.ForceVelocity * Quaternion.Inverse(Prim.ForceOrientation); // DEBUG:
561 // END DEBUG
562
563 m_vehicleMass = Prim.Linkset.LinksetMass;
564
565 MoveLinear(pTimestep); 593 MoveLinear(pTimestep);
566 // Commented out for debug
567 MoveAngular(pTimestep); 594 MoveAngular(pTimestep);
568 // Prim.ApplyTorqueImpulse(-Prim.RotationalVelocity * m_vehicleMass, false); // DEBUG DEBUG
569 // Prim.ForceRotationalVelocity = -Prim.RotationalVelocity; // DEBUG DEBUG
570 595
571 LimitRotation(pTimestep); 596 LimitRotation(pTimestep);
572 597
573 // remember the position so next step we can limit absolute movement effects 598 // remember the position so next step we can limit absolute movement effects
574 m_lastPositionVector = Prim.ForcePosition; 599 m_lastPositionVector = Prim.ForcePosition;
575 600
576 VDetailLog("{0},BSDynamics.Step,frict={1},grav={2},inertia={3},mass={4}", // DEBUG DEBUG
577 Prim.LocalID,
578 BulletSimAPI.GetFriction2(Prim.PhysBody.ptr),
579 BulletSimAPI.GetGravity2(Prim.PhysBody.ptr),
580 Prim.Inertia,
581 m_vehicleMass
582 );
583 VDetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}", 601 VDetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}",
584 Prim.LocalID, Prim.ForcePosition, Prim.Force, Prim.ForceVelocity, Prim.RotationalVelocity); 602 Prim.LocalID, Prim.ForcePosition, Prim.Force, Prim.ForceVelocity, Prim.RotationalVelocity);
585 }// end Step 603 }
586 604
587 // Apply the effect of the linear motor. 605 // Apply the effect of the linear motor.
588 // Also does hover and float. 606 // Also does hover and float.
589 private void MoveLinear(float pTimestep) 607 private void MoveLinear(float pTimestep)
590 { 608 {
591 // m_linearMotorDirection is the target direction we are moving relative to the vehicle coordinates 609 Vector3 linearMotorContribution = m_linearMotor.Step(pTimestep);
592 // m_lastLinearVelocityVector is the current speed we are moving in that direction
593 if (m_linearMotorDirection.LengthSquared() > 0.001f)
594 {
595 Vector3 origDir = m_linearMotorDirection; // DEBUG
596 Vector3 origVel = m_lastLinearVelocityVector; // DEBUG
597 // DEBUG: the vehicle velocity rotated to be relative to vehicle coordinates for comparison
598 Vector3 vehicleVelocity = Prim.ForceVelocity * Quaternion.Inverse(Prim.ForceOrientation); // DEBUG
599
600 // Add (desiredVelocity - lastAppliedVelocity) / howLongItShouldTakeToComplete
601 Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/(m_linearMotorTimescale) * pTimestep;
602 m_lastLinearVelocityVector += addAmount;
603
604 float decayFactor = (1.0f / m_linearMotorDecayTimescale) * pTimestep;
605 m_linearMotorDirection *= (1f - decayFactor);
606
607 // Rotate new object velocity from vehicle relative to world coordinates
608 m_newVelocity = m_lastLinearVelocityVector * Prim.ForceOrientation;
609
610 // Apply friction for next time
611 Vector3 frictionFactor = (Vector3.One / m_linearFrictionTimescale) * pTimestep;
612 m_lastLinearVelocityVector *= (Vector3.One - frictionFactor);
613
614 VDetailLog("{0},MoveLinear,nonZero,origlmDir={1},origlvVel={2},vehVel={3},add={4},decay={5},frict={6},lmDir={7},lvVec={8},newVel={9}",
615 Prim.LocalID, origDir, origVel, vehicleVelocity, addAmount, decayFactor, frictionFactor,
616 m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity);
617 }
618 else
619 {
620 // if what remains of direction is very small, zero it.
621 m_linearMotorDirection = Vector3.Zero;
622 m_lastLinearVelocityVector = Vector3.Zero;
623 m_newVelocity = Vector3.Zero;
624
625 VDetailLog("{0},MoveLinear,zeroed", Prim.LocalID);
626 }
627 610
628 // m_newVelocity is velocity computed from linear motor in world coordinates 611 // Rotate new object velocity from vehicle relative to world coordinates
612 linearMotorContribution *= Prim.ForceOrientation;
629 613
614 // ==================================================================
630 // Gravity and Buoyancy 615 // Gravity and Buoyancy
631 // There is some gravity, make a gravity force vector that is applied after object velocity. 616 // There is some gravity, make a gravity force vector that is applied after object velocity.
632 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; 617 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
633 Vector3 grav = Prim.PhysicsScene.DefaultGravity * (1f - m_VehicleBuoyancy); 618 Vector3 grav = Prim.PhysicsScene.DefaultGravity * (1f - m_VehicleBuoyancy);
634 619
635 /* 620 // Current vehicle position
636 * RA: Not sure why one would do this unless we are hoping external forces are doing gravity, ...
637 // Preserve the current Z velocity
638 Vector3 vel_now = m_prim.Velocity;
639 m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity
640 */
641
642 Vector3 pos = Prim.ForcePosition; 621 Vector3 pos = Prim.ForcePosition;
643// Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f);
644 622
623 // ==================================================================
624 Vector3 terrainHeightContribution = Vector3.Zero;
645 // If below the terrain, move us above the ground a little. 625 // If below the terrain, move us above the ground a little.
646 float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos); 626 float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
647 // Taking the rotated size doesn't work here because m_prim.Size is the size of the root prim and not the linkset. 627 // Taking the rotated size doesn't work here because m_prim.Size is the size of the root prim and not the linkset.
@@ -650,11 +630,14 @@ namespace OpenSim.Region.Physics.BulletSPlugin
650 // if (rotatedSize.Z < terrainHeight) 630 // if (rotatedSize.Z < terrainHeight)
651 if (pos.Z < terrainHeight) 631 if (pos.Z < terrainHeight)
652 { 632 {
633 // TODO: correct position by applying force rather than forcing position.
653 pos.Z = terrainHeight + 2; 634 pos.Z = terrainHeight + 2;
654 Prim.ForcePosition = pos; 635 Prim.ForcePosition = pos;
655 VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, terrainHeight, pos); 636 VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, terrainHeight, pos);
656 } 637 }
657 638
639 // ==================================================================
640 Vector3 hoverContribution = Vector3.Zero;
658 // Check if hovering 641 // Check if hovering
659 // m_VhoverEfficiency: 0=bouncy, 1=totally damped 642 // m_VhoverEfficiency: 0=bouncy, 1=totally damped
660 // m_VhoverTimescale: time to achieve height 643 // m_VhoverTimescale: time to achieve height
@@ -694,24 +677,22 @@ namespace OpenSim.Region.Physics.BulletSPlugin
694 // RA: where does the 50 come from? 677 // RA: where does the 50 come from?
695 float verticalCorrectionVelocity = pTimestep * ((verticalError * 50.0f) / m_VhoverTimescale); 678 float verticalCorrectionVelocity = pTimestep * ((verticalError * 50.0f) / m_VhoverTimescale);
696 // Replace Vertical speed with correction figure if significant 679 // Replace Vertical speed with correction figure if significant
697 if (Math.Abs(verticalError) > 0.01f) 680 if (verticalError > 0.01f)
698 { 681 {
699 m_newVelocity.Z += verticalCorrectionVelocity; 682 hoverContribution = new Vector3(0f, 0f, verticalCorrectionVelocity);
700 //KF: m_VhoverEfficiency is not yet implemented 683 //KF: m_VhoverEfficiency is not yet implemented
701 } 684 }
702 else if (verticalError < -0.01) 685 else if (verticalError < -0.01)
703 { 686 {
704 m_newVelocity.Z -= verticalCorrectionVelocity; 687 hoverContribution = new Vector3(0f, 0f, -verticalCorrectionVelocity);
705 }
706 else
707 {
708 m_newVelocity.Z = 0f;
709 } 688 }
710 } 689 }
711 690
712 VDetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}", Prim.LocalID, pos, m_newVelocity, m_VhoverHeight, m_VhoverTargetHeight); 691 VDetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}",
692 Prim.LocalID, pos, hoverContribution, m_VhoverHeight, m_VhoverTargetHeight);
713 } 693 }
714 694
695 // ==================================================================
715 Vector3 posChange = pos - m_lastPositionVector; 696 Vector3 posChange = pos - m_lastPositionVector;
716 if (m_BlockingEndPoint != Vector3.Zero) 697 if (m_BlockingEndPoint != Vector3.Zero)
717 { 698 {
@@ -749,70 +730,77 @@ namespace OpenSim.Region.Physics.BulletSPlugin
749 } 730 }
750 } 731 }
751 732
752 #region downForce 733 // ==================================================================
753 Vector3 downForce = Vector3.Zero; 734 Vector3 limitMotorUpContribution = Vector3.Zero;
754
755 if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0) 735 if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
756 { 736 {
757 // If the vehicle is motoring into the sky, get it going back down. 737 // If the vehicle is motoring into the sky, get it going back down.
758 // Is this an angular force or both linear and angular??
759 float distanceAboveGround = pos.Z - terrainHeight; 738 float distanceAboveGround = pos.Z - terrainHeight;
760 if (distanceAboveGround > 2f) 739 if (distanceAboveGround > 1f)
761 { 740 {
762 // downForce = new Vector3(0, 0, (-distanceAboveGround / m_bankingTimescale) * pTimestep); 741 // downForce = new Vector3(0, 0, (-distanceAboveGround / m_bankingTimescale) * pTimestep);
763 // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale); 742 // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale);
764 downForce = new Vector3(0, 0, -distanceAboveGround); 743 limitMotorUpContribution = new Vector3(0, 0, -distanceAboveGround);
765 } 744 }
766 // TODO: this calculation is all wrong. From the description at 745 // TODO: this calculation is all wrong. From the description at
767 // (http://wiki.secondlife.com/wiki/Category:LSL_Vehicle), the downForce 746 // (http://wiki.secondlife.com/wiki/Category:LSL_Vehicle), the downForce
768 // has a decay factor. This says this force should 747 // has a decay factor. This says this force should
769 // be computed with a motor. 748 // be computed with a motor.
770 VDetailLog("{0},MoveLinear,limitMotorUp,distAbove={1},downForce={2}", 749 VDetailLog("{0},MoveLinear,limitMotorUp,distAbove={1},downForce={2}",
771 Prim.LocalID, distanceAboveGround, downForce); 750 Prim.LocalID, distanceAboveGround, limitMotorUpContribution);
772 } 751 }
773 #endregion // downForce 752
753 // ==================================================================
754 Vector3 newVelocity = linearMotorContribution
755 + terrainHeightContribution
756 + hoverContribution
757 + limitMotorUpContribution;
774 758
775 // If not changing some axis, reduce out velocity 759 // If not changing some axis, reduce out velocity
776 if ((m_flags & (VehicleFlag.NO_X)) != 0) 760 if ((m_flags & (VehicleFlag.NO_X)) != 0)
777 m_newVelocity.X = 0; 761 newVelocity.X = 0;
778 if ((m_flags & (VehicleFlag.NO_Y)) != 0) 762 if ((m_flags & (VehicleFlag.NO_Y)) != 0)
779 m_newVelocity.Y = 0; 763 newVelocity.Y = 0;
780 if ((m_flags & (VehicleFlag.NO_Z)) != 0) 764 if ((m_flags & (VehicleFlag.NO_Z)) != 0)
781 m_newVelocity.Z = 0; 765 newVelocity.Z = 0;
782 766
767 // ==================================================================
783 // Clamp REALLY high or low velocities 768 // Clamp REALLY high or low velocities
784 if (m_newVelocity.LengthSquared() > 1e6f) 769 float newVelocityLengthSq = newVelocity.LengthSquared();
770 if (newVelocityLengthSq > 1e6f)
785 { 771 {
786 m_newVelocity /= m_newVelocity.Length(); 772 newVelocity /= newVelocity.Length();
787 m_newVelocity *= 1000f; 773 newVelocity *= 1000f;
788 } 774 }
789 else if (m_newVelocity.LengthSquared() < 1e-6f) 775 else if (newVelocityLengthSq < 1e-6f)
790 m_newVelocity = Vector3.Zero; 776 newVelocity = Vector3.Zero;
791 777
778 // ==================================================================
792 // Stuff new linear velocity into the vehicle 779 // Stuff new linear velocity into the vehicle
793 Prim.ForceVelocity = m_newVelocity; 780 Prim.ForceVelocity = newVelocity;
794 // Prim.ApplyForceImpulse((m_newVelocity - Prim.Velocity) * m_vehicleMass, false); // DEBUG DEBUG 781 // Prim.ApplyForceImpulse((m_newVelocity - Prim.Velocity) * m_vehicleMass, false); // DEBUG DEBUG
795 782
796 Vector3 totalDownForce = downForce + grav; 783 // Other linear forces are applied as forces.
784 Vector3 totalDownForce = grav * m_vehicleMass;
797 if (totalDownForce != Vector3.Zero) 785 if (totalDownForce != Vector3.Zero)
798 { 786 {
799 Prim.AddForce(totalDownForce * m_vehicleMass, false); 787 Prim.AddForce(totalDownForce, false);
800 // Prim.ApplyForceImpulse(totalDownForce * m_vehicleMass, false);
801 } 788 }
802 789
803 VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},primVel={4},totalDown={5}", 790 VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},primVel={4},totalDown={5}",
804 Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity, Prim.Velocity, totalDownForce); 791 Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector,
792 newVelocity, Prim.Velocity, totalDownForce);
805 793
806 } // end MoveLinear() 794 } // end MoveLinear()
807 795
808 // ======================================================================= 796 // =======================================================================
797 // =======================================================================
809 // Apply the effect of the angular motor. 798 // Apply the effect of the angular motor.
810 private void MoveAngular(float pTimestep) 799 private void MoveAngular(float pTimestep)
811 { 800 {
812 // m_angularMotorDirection // angular velocity requested by LSL motor 801 // m_angularMotorDirection // angular velocity requested by LSL motor
813 // m_angularMotorApply // application frame counter
814 // m_angularMotorVelocity // current angular motor velocity (ramps up and down) 802 // m_angularMotorVelocity // current angular motor velocity (ramps up and down)
815 // m_angularMotorTimescale // motor angular velocity ramp up rate 803 // m_angularMotorTimescale // motor angular velocity ramp up time
816 // m_angularMotorDecayTimescale // motor angular velocity decay rate 804 // m_angularMotorDecayTimescale // motor angular velocity decay rate
817 // m_angularFrictionTimescale // body angular velocity decay rate 805 // m_angularFrictionTimescale // body angular velocity decay rate
818 // m_lastAngularVelocity // what was last applied to body 806 // m_lastAngularVelocity // what was last applied to body
@@ -836,18 +824,16 @@ namespace OpenSim.Region.Physics.BulletSPlugin
836 m_angularMotorVelocity = Vector3.Zero; 824 m_angularMotorVelocity = Vector3.Zero;
837 } 825 }
838 826
839 #region Vertical attactor 827 Vector3 angularMotorContribution = m_angularMotor.Step(pTimestep);
840
841 Vector3 vertattr = Vector3.Zero;
842 Vector3 deflection = Vector3.Zero;
843 Vector3 banking = Vector3.Zero;
844 828
829 // ==================================================================
830 Vector3 verticalAttractionContribution = Vector3.Zero;
845 // If vertical attaction timescale is reasonable and we applied an angular force last time... 831 // If vertical attaction timescale is reasonable and we applied an angular force last time...
846 if (m_verticalAttractionTimescale < 300 && m_lastAngularVelocity != Vector3.Zero) 832 if (m_verticalAttractionTimescale < 300 && m_lastAngularVelocity != Vector3.Zero)
847 { 833 {
848 float VAservo = pTimestep * 0.2f / m_verticalAttractionTimescale; 834 float VAservo = pTimestep * 0.2f / m_verticalAttractionTimescale;
849 if (Prim.IsColliding) 835 if (Prim.IsColliding)
850 VAservo = pTimestep * 0.05f / (m_verticalAttractionTimescale); 836 VAservo = pTimestep * 0.05f / m_verticalAttractionTimescale;
851 837
852 VAservo *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency); 838 VAservo *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
853 839
@@ -871,24 +857,23 @@ namespace OpenSim.Region.Physics.BulletSPlugin
871 // As the body rotates around the X axis, then verticalError.Y increases; Rotated around Y 857 // As the body rotates around the X axis, then verticalError.Y increases; Rotated around Y
872 // then .X increases, so change Body angular velocity X based on Y, and Y based on X. 858 // then .X increases, so change Body angular velocity X based on Y, and Y based on X.
873 // Z is not changed. 859 // Z is not changed.
874 vertattr.X = verticalError.Y; 860 verticalAttractionContribution.X = verticalError.Y;
875 vertattr.Y = - verticalError.X; 861 verticalAttractionContribution.Y = - verticalError.X;
876 vertattr.Z = 0f; 862 verticalAttractionContribution.Z = 0f;
877 863
878 // scaling appears better usingsquare-law 864 // scaling appears better usingsquare-law
879 Vector3 angularVelocity = Prim.ForceRotationalVelocity; 865 Vector3 angularVelocity = Prim.ForceRotationalVelocity;
880 float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency); 866 float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
881 vertattr.X += bounce * angularVelocity.X; 867 verticalAttractionContribution.X += bounce * angularVelocity.X;
882 vertattr.Y += bounce * angularVelocity.Y; 868 verticalAttractionContribution.Y += bounce * angularVelocity.Y;
883 869
884 VDetailLog("{0},MoveAngular,verticalAttraction,VAservo={1},effic={2},verticalError={3},bounce={4},vertattr={5}", 870 VDetailLog("{0},MoveAngular,verticalAttraction,VAservo={1},effic={2},verticalError={3},bounce={4},vertattr={5}",
885 Prim.LocalID, VAservo, m_verticalAttractionEfficiency, verticalError, bounce, vertattr); 871 Prim.LocalID, VAservo, m_verticalAttractionEfficiency, verticalError, bounce, verticalAttractionContribution);
886 872
887 } 873 }
888 #endregion // Vertical attactor
889
890 #region Deflection
891 874
875 // ==================================================================
876 Vector3 deflectionContribution = Vector3.Zero;
892 if (m_angularDeflectionEfficiency != 0) 877 if (m_angularDeflectionEfficiency != 0)
893 { 878 {
894 // Compute a scaled vector that points in the preferred axis (X direction) 879 // Compute a scaled vector that points in the preferred axis (X direction)
@@ -899,18 +884,16 @@ namespace OpenSim.Region.Physics.BulletSPlugin
899 Vector3 preferredAxisOfMotion = scaledDefaultDirection * Quaternion.Add(Prim.ForceOrientation, m_referenceFrame); 884 Vector3 preferredAxisOfMotion = scaledDefaultDirection * Quaternion.Add(Prim.ForceOrientation, m_referenceFrame);
900 885
901 // Scale by efficiency and timescale 886 // Scale by efficiency and timescale
902 deflection = (preferredAxisOfMotion * (m_angularDeflectionEfficiency) / m_angularDeflectionTimescale) * pTimestep; 887 deflectionContribution = (preferredAxisOfMotion * (m_angularDeflectionEfficiency) / m_angularDeflectionTimescale) * pTimestep;
903 888
904 VDetailLog("{0},MoveAngular,Deflection,perfAxis={1},deflection={2}", 889 VDetailLog("{0},MoveAngular,Deflection,perfAxis={1},deflection={2}",
905 Prim.LocalID, preferredAxisOfMotion, deflection); 890 Prim.LocalID, preferredAxisOfMotion, deflectionContribution);
906 // This deflection computation is not correct. 891 // This deflection computation is not correct.
907 deflection = Vector3.Zero; 892 deflectionContribution = Vector3.Zero;
908 } 893 }
909 894
910 #endregion 895 // ==================================================================
911 896 Vector3 bankingContribution = Vector3.Zero;
912 #region Banking
913
914 if (m_bankingEfficiency != 0) 897 if (m_bankingEfficiency != 0)
915 { 898 {
916 Vector3 dir = Vector3.One * Prim.ForceOrientation; 899 Vector3 dir = Vector3.One * Prim.ForceOrientation;
@@ -925,6 +908,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
925 float mix = Math.Abs(m_bankingMix); 908 float mix = Math.Abs(m_bankingMix);
926 if (m_angularMotorVelocity.X == 0) 909 if (m_angularMotorVelocity.X == 0)
927 { 910 {
911 // The vehicle is stopped
928 /*if (!parent.Orientation.ApproxEquals(this.m_referenceFrame, 0.25f)) 912 /*if (!parent.Orientation.ApproxEquals(this.m_referenceFrame, 0.25f))
929 { 913 {
930 Vector3 axisAngle; 914 Vector3 axisAngle;
@@ -938,9 +922,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
938 }*/ 922 }*/
939 } 923 }
940 else 924 else
941 banking.Z += (effSquared*(mult*mix))*(m_angularMotorVelocity.X) * 4; 925 {
926 bankingContribution.Z += (effSquared * (mult * mix)) * (m_angularMotorVelocity.X) * 4;
927 }
928
929 //If they are colliding, we probably shouldn't shove the prim around... probably
942 if (!Prim.IsColliding && Math.Abs(m_angularMotorVelocity.X) > mix) 930 if (!Prim.IsColliding && Math.Abs(m_angularMotorVelocity.X) > mix)
943 //If they are colliding, we probably shouldn't shove the prim around... probably
944 { 931 {
945 float angVelZ = m_angularMotorVelocity.X*-1; 932 float angVelZ = m_angularMotorVelocity.X*-1;
946 /*if(angVelZ > mix) 933 /*if(angVelZ > mix)
@@ -954,22 +941,23 @@ namespace OpenSim.Region.Physics.BulletSPlugin
954 else if (bankingRot.X < -3) 941 else if (bankingRot.X < -3)
955 bankingRot.X = -3; 942 bankingRot.X = -3;
956 bankingRot *= Prim.ForceOrientation; 943 bankingRot *= Prim.ForceOrientation;
957 banking += bankingRot; 944 bankingContribution += bankingRot;
958 } 945 }
959 m_angularMotorVelocity.X *= m_bankingEfficiency == 1 ? 0.0f : 1 - m_bankingEfficiency; 946 m_angularMotorVelocity.X *= m_bankingEfficiency == 1 ? 0.0f : 1 - m_bankingEfficiency;
960 VDetailLog("{0},MoveAngular,Banking,bEff={1},angMotVel={2},banking={3}", 947 VDetailLog("{0},MoveAngular,Banking,bEff={1},angMotVel={2},effSq={3},mult={4},mix={5},banking={6}",
961 Prim.LocalID, m_bankingEfficiency, m_angularMotorVelocity, banking); 948 Prim.LocalID, m_bankingEfficiency, m_angularMotorVelocity, effSquared, mult, mix, bankingContribution);
962 } 949 }
963 950
964 #endregion 951 // ==================================================================
965 952 m_lastVertAttractor = verticalAttractionContribution;
966 m_lastVertAttractor = vertattr;
967 953
968 // Sum velocities 954 // Sum velocities
969 m_lastAngularVelocity = m_angularMotorVelocity + vertattr + banking + deflection; 955 m_lastAngularVelocity = angularMotorContribution
970 956 + verticalAttractionContribution
971 #region Linear Motor Offset 957 + bankingContribution
958 + deflectionContribution;
972 959
960 // ==================================================================
973 //Offset section 961 //Offset section
974 if (m_linearMotorOffset != Vector3.Zero) 962 if (m_linearMotorOffset != Vector3.Zero)
975 { 963 {
@@ -985,8 +973,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
985 // 973 //
986 // The torque created is the linear velocity crossed with the offset 974 // The torque created is the linear velocity crossed with the offset
987 975
988 // NOTE: this computation does should be in the linear section 976 // TODO: this computation should be in the linear section
989 // because there we know the impulse being applied. 977 // because that is where we know the impulse being applied.
990 Vector3 torqueFromOffset = Vector3.Zero; 978 Vector3 torqueFromOffset = Vector3.Zero;
991 // torqueFromOffset = Vector3.Cross(m_linearMotorOffset, appliedImpulse); 979 // torqueFromOffset = Vector3.Cross(m_linearMotorOffset, appliedImpulse);
992 if (float.IsNaN(torqueFromOffset.X)) 980 if (float.IsNaN(torqueFromOffset.X))
@@ -1000,8 +988,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
1000 VDetailLog("{0},BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset); 988 VDetailLog("{0},BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset);
1001 } 989 }
1002 990
1003 #endregion 991 // ==================================================================
1004 992 // NO_DEFLECTION_UP says angular motion should not add any pitch or roll movement
1005 if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0) 993 if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
1006 { 994 {
1007 m_lastAngularVelocity.X = 0; 995 m_lastAngularVelocity.X = 0;
@@ -1009,6 +997,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
1009 VDetailLog("{0},MoveAngular,noDeflectionUp,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity); 997 VDetailLog("{0},MoveAngular,noDeflectionUp,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
1010 } 998 }
1011 999
1000 // ==================================================================
1012 if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f)) 1001 if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
1013 { 1002 {
1014 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero. 1003 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
@@ -1021,18 +1010,20 @@ namespace OpenSim.Region.Physics.BulletSPlugin
1021 // The above calculates the absolute angular velocity needed. Angular velocity is massless. 1010 // The above calculates the absolute angular velocity needed. Angular velocity is massless.
1022 // Since we are stuffing the angular velocity directly into the object, the computed 1011 // Since we are stuffing the angular velocity directly into the object, the computed
1023 // velocity needs to be scaled by the timestep. 1012 // velocity needs to be scaled by the timestep.
1024 Vector3 applyAngularForce = ((m_lastAngularVelocity * pTimestep) - Prim.ForceRotationalVelocity); 1013 // Also remove any motion that is on the object so added motion is only from vehicle.
1014 Vector3 applyAngularForce = ((m_lastAngularVelocity * pTimestep)
1015 - Prim.ForceRotationalVelocity);
1025 Prim.ForceRotationalVelocity = applyAngularForce; 1016 Prim.ForceRotationalVelocity = applyAngularForce;
1026 1017
1027 // Decay the angular movement for next time 1018 VDetailLog("{0},MoveAngular,done,newRotVel={1},lastAngular={2}",
1028 Vector3 decayamount = (Vector3.One / m_angularFrictionTimescale) * pTimestep; 1019 Prim.LocalID, applyAngularForce, m_lastAngularVelocity);
1029 m_lastAngularVelocity *= Vector3.One - decayamount;
1030
1031 VDetailLog("{0},MoveAngular,done,newRotVel={1},decay={2},lastAngular={3}",
1032 Prim.LocalID, applyAngularForce, decayamount, m_lastAngularVelocity);
1033 } 1020 }
1034 } //end MoveAngular 1021 }
1035 1022
1023 // This is from previous instantiations of XXXDynamics.cs.
1024 // Applies roll reference frame.
1025 // TODO: is this the right way to separate the code to do this operation?
1026 // Should this be in MoveAngular()?
1036 internal void LimitRotation(float timestep) 1027 internal void LimitRotation(float timestep)
1037 { 1028 {
1038 Quaternion rotq = Prim.ForceOrientation; 1029 Quaternion rotq = Prim.ForceOrientation;