diff options
Diffstat (limited to '')
-rw-r--r-- | OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs | 599 |
1 files changed, 335 insertions, 264 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs index dbc9039..3a73fba 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 |
@@ -124,6 +125,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
124 | // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. | 125 | // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. |
125 | 126 | ||
126 | //Attractor properties | 127 | //Attractor properties |
128 | private BSVMotor m_verticalAttractionMotor = new BSVMotor("VerticalAttraction"); | ||
127 | private float m_verticalAttractionEfficiency = 1.0f; // damped | 129 | private float m_verticalAttractionEfficiency = 1.0f; // damped |
128 | private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor. | 130 | private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor. |
129 | 131 | ||
@@ -152,10 +154,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
152 | m_angularDeflectionTimescale = Math.Max(pValue, 0.01f); | 154 | m_angularDeflectionTimescale = Math.Max(pValue, 0.01f); |
153 | break; | 155 | break; |
154 | case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: | 156 | case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: |
155 | m_angularMotorDecayTimescale = Math.Max(pValue, 0.01f); | 157 | m_angularMotorDecayTimescale = Math.Max(0.01f, Math.Min(pValue,120)); |
158 | m_angularMotor.TargetValueDecayTimeScale = m_angularMotorDecayTimescale; | ||
156 | break; | 159 | break; |
157 | case Vehicle.ANGULAR_MOTOR_TIMESCALE: | 160 | case Vehicle.ANGULAR_MOTOR_TIMESCALE: |
158 | m_angularMotorTimescale = Math.Max(pValue, 0.01f); | 161 | m_angularMotorTimescale = Math.Max(pValue, 0.01f); |
162 | m_angularMotor.TimeScale = m_angularMotorTimescale; | ||
159 | break; | 163 | break; |
160 | case Vehicle.BANKING_EFFICIENCY: | 164 | case Vehicle.BANKING_EFFICIENCY: |
161 | m_bankingEfficiency = Math.Max(-1f, Math.Min(pValue, 1f)); | 165 | m_bankingEfficiency = Math.Max(-1f, Math.Min(pValue, 1f)); |
@@ -185,33 +189,40 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
185 | m_linearDeflectionTimescale = Math.Max(pValue, 0.01f); | 189 | m_linearDeflectionTimescale = Math.Max(pValue, 0.01f); |
186 | break; | 190 | break; |
187 | case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: | 191 | case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: |
188 | m_linearMotorDecayTimescale = Math.Max(pValue, 0.01f); | 192 | m_linearMotorDecayTimescale = Math.Max(0.01f, Math.Min(pValue,120)); |
193 | m_linearMotor.TargetValueDecayTimeScale = m_linearMotorDecayTimescale; | ||
189 | break; | 194 | break; |
190 | case Vehicle.LINEAR_MOTOR_TIMESCALE: | 195 | case Vehicle.LINEAR_MOTOR_TIMESCALE: |
191 | m_linearMotorTimescale = Math.Max(pValue, 0.01f); | 196 | m_linearMotorTimescale = Math.Max(pValue, 0.01f); |
197 | m_linearMotor.TimeScale = m_linearMotorTimescale; | ||
192 | break; | 198 | break; |
193 | case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: | 199 | case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: |
194 | m_verticalAttractionEfficiency = Math.Max(0.1f, Math.Min(pValue, 1f)); | 200 | m_verticalAttractionEfficiency = Math.Max(0.1f, Math.Min(pValue, 1f)); |
201 | m_verticalAttractionMotor.Efficiency = m_verticalAttractionEfficiency; | ||
195 | break; | 202 | break; |
196 | case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: | 203 | case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: |
197 | m_verticalAttractionTimescale = Math.Max(pValue, 0.01f); | 204 | m_verticalAttractionTimescale = Math.Max(pValue, 0.01f); |
205 | m_verticalAttractionMotor.TimeScale = m_verticalAttractionTimescale; | ||
198 | break; | 206 | break; |
199 | 207 | ||
200 | // These are vector properties but the engine lets you use a single float value to | 208 | // These are vector properties but the engine lets you use a single float value to |
201 | // set all of the components to the same value | 209 | // set all of the components to the same value |
202 | case Vehicle.ANGULAR_FRICTION_TIMESCALE: | 210 | case Vehicle.ANGULAR_FRICTION_TIMESCALE: |
203 | m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); | 211 | m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); |
212 | m_angularMotor.FrictionTimescale = m_angularFrictionTimescale; | ||
204 | break; | 213 | break; |
205 | case Vehicle.ANGULAR_MOTOR_DIRECTION: | 214 | case Vehicle.ANGULAR_MOTOR_DIRECTION: |
206 | m_angularMotorDirection = new Vector3(pValue, pValue, pValue); | 215 | m_angularMotorDirection = new Vector3(pValue, pValue, pValue); |
207 | // m_angularMotorApply = 100; | 216 | m_angularMotor.SetTarget(m_angularMotorDirection); |
208 | break; | 217 | break; |
209 | case Vehicle.LINEAR_FRICTION_TIMESCALE: | 218 | case Vehicle.LINEAR_FRICTION_TIMESCALE: |
210 | m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); | 219 | m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); |
220 | m_linearMotor.FrictionTimescale = m_linearFrictionTimescale; | ||
211 | break; | 221 | break; |
212 | case Vehicle.LINEAR_MOTOR_DIRECTION: | 222 | case Vehicle.LINEAR_MOTOR_DIRECTION: |
213 | m_linearMotorDirection = new Vector3(pValue, pValue, pValue); | 223 | m_linearMotorDirection = new Vector3(pValue, pValue, pValue); |
214 | m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue); | 224 | m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue); |
225 | m_linearMotor.SetTarget(m_linearMotorDirection); | ||
215 | break; | 226 | break; |
216 | case Vehicle.LINEAR_MOTOR_OFFSET: | 227 | case Vehicle.LINEAR_MOTOR_OFFSET: |
217 | m_linearMotorOffset = new Vector3(pValue, pValue, pValue); | 228 | m_linearMotorOffset = new Vector3(pValue, pValue, pValue); |
@@ -227,6 +238,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
227 | { | 238 | { |
228 | case Vehicle.ANGULAR_FRICTION_TIMESCALE: | 239 | case Vehicle.ANGULAR_FRICTION_TIMESCALE: |
229 | m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); | 240 | m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); |
241 | m_angularMotor.FrictionTimescale = m_angularFrictionTimescale; | ||
230 | break; | 242 | break; |
231 | case Vehicle.ANGULAR_MOTOR_DIRECTION: | 243 | case Vehicle.ANGULAR_MOTOR_DIRECTION: |
232 | // Limit requested angular speed to 2 rps= 4 pi rads/sec | 244 | // Limit requested angular speed to 2 rps= 4 pi rads/sec |
@@ -234,14 +246,16 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
234 | pValue.Y = Math.Max(-12.56f, Math.Min(pValue.Y, 12.56f)); | 246 | 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)); | 247 | pValue.Z = Math.Max(-12.56f, Math.Min(pValue.Z, 12.56f)); |
236 | m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); | 248 | m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); |
237 | // m_angularMotorApply = 100; | 249 | m_angularMotor.SetTarget(m_angularMotorDirection); |
238 | break; | 250 | break; |
239 | case Vehicle.LINEAR_FRICTION_TIMESCALE: | 251 | case Vehicle.LINEAR_FRICTION_TIMESCALE: |
240 | m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); | 252 | m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); |
253 | m_linearMotor.FrictionTimescale = m_linearFrictionTimescale; | ||
241 | break; | 254 | break; |
242 | case Vehicle.LINEAR_MOTOR_DIRECTION: | 255 | case Vehicle.LINEAR_MOTOR_DIRECTION: |
243 | m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); | 256 | m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); |
244 | m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); | 257 | m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); |
258 | m_linearMotor.SetTarget(m_linearMotorDirection); | ||
245 | break; | 259 | break; |
246 | case Vehicle.LINEAR_MOTOR_OFFSET: | 260 | case Vehicle.LINEAR_MOTOR_OFFSET: |
247 | m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); | 261 | m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); |
@@ -303,7 +317,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
303 | m_VhoverEfficiency = 0; | 317 | m_VhoverEfficiency = 0; |
304 | m_VhoverTimescale = 0; | 318 | m_VhoverTimescale = 0; |
305 | m_VehicleBuoyancy = 0; | 319 | m_VehicleBuoyancy = 0; |
306 | 320 | ||
307 | m_linearDeflectionEfficiency = 1; | 321 | m_linearDeflectionEfficiency = 1; |
308 | m_linearDeflectionTimescale = 1; | 322 | m_linearDeflectionTimescale = 1; |
309 | 323 | ||
@@ -319,6 +333,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
319 | 333 | ||
320 | m_referenceFrame = Quaternion.Identity; | 334 | m_referenceFrame = Quaternion.Identity; |
321 | m_flags = (VehicleFlag)0; | 335 | m_flags = (VehicleFlag)0; |
336 | |||
322 | break; | 337 | break; |
323 | 338 | ||
324 | case Vehicle.TYPE_SLED: | 339 | case Vehicle.TYPE_SLED: |
@@ -351,10 +366,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
351 | m_bankingMix = 1; | 366 | m_bankingMix = 1; |
352 | 367 | ||
353 | m_referenceFrame = Quaternion.Identity; | 368 | m_referenceFrame = Quaternion.Identity; |
354 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP); | 369 | m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
355 | m_flags &= | 370 | | VehicleFlag.HOVER_TERRAIN_ONLY |
356 | ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | | 371 | | VehicleFlag.HOVER_GLOBAL_HEIGHT |
357 | VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); | 372 | | VehicleFlag.HOVER_UP_ONLY); |
373 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP | ||
374 | | VehicleFlag.LIMIT_ROLL_ONLY | ||
375 | | VehicleFlag.LIMIT_MOTOR_UP); | ||
358 | break; | 376 | break; |
359 | case Vehicle.TYPE_CAR: | 377 | case Vehicle.TYPE_CAR: |
360 | m_linearMotorDirection = Vector3.Zero; | 378 | m_linearMotorDirection = Vector3.Zero; |
@@ -427,9 +445,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
427 | m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | 445 | m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY |
428 | | VehicleFlag.HOVER_GLOBAL_HEIGHT | 446 | | VehicleFlag.HOVER_GLOBAL_HEIGHT |
429 | | VehicleFlag.LIMIT_ROLL_ONLY | 447 | | VehicleFlag.LIMIT_ROLL_ONLY |
448 | | VehicleFlag.LIMIT_MOTOR_UP | ||
430 | | VehicleFlag.HOVER_UP_ONLY); | 449 | | VehicleFlag.HOVER_UP_ONLY); |
431 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP | 450 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
432 | | VehicleFlag.LIMIT_MOTOR_UP | ||
433 | | VehicleFlag.HOVER_WATER_ONLY); | 451 | | VehicleFlag.HOVER_WATER_ONLY); |
434 | break; | 452 | break; |
435 | case Vehicle.TYPE_AIRPLANE: | 453 | case Vehicle.TYPE_AIRPLANE: |
@@ -510,6 +528,28 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
510 | | VehicleFlag.HOVER_GLOBAL_HEIGHT); | 528 | | VehicleFlag.HOVER_GLOBAL_HEIGHT); |
511 | break; | 529 | break; |
512 | } | 530 | } |
531 | |||
532 | // Update any physical parameters based on this type. | ||
533 | Refresh(); | ||
534 | |||
535 | m_linearMotor = new BSVMotor("LinearMotor", m_linearMotorTimescale, | ||
536 | m_linearMotorDecayTimescale, m_linearFrictionTimescale, | ||
537 | 1f); | ||
538 | m_linearMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG DEBUG (enables detail logging) | ||
539 | |||
540 | m_angularMotor = new BSVMotor("AngularMotor", m_angularMotorTimescale, | ||
541 | m_angularMotorDecayTimescale, m_angularFrictionTimescale, | ||
542 | 1f); | ||
543 | m_angularMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG DEBUG (enables detail logging) | ||
544 | |||
545 | m_verticalAttractionMotor = new BSVMotor("VerticalAttraction", m_verticalAttractionTimescale, | ||
546 | BSMotor.Infinite, BSMotor.InfiniteVector, | ||
547 | m_verticalAttractionEfficiency); | ||
548 | // Z goes away and we keep X and Y | ||
549 | m_verticalAttractionMotor.FrictionTimescale = new Vector3(BSMotor.Infinite, BSMotor.Infinite, 0.1f); | ||
550 | m_verticalAttractionMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG DEBUG (enables detail logging) | ||
551 | |||
552 | // m_bankingMotor = new BSVMotor("BankingMotor", ...); | ||
513 | } | 553 | } |
514 | 554 | ||
515 | // Some of the properties of this prim may have changed. | 555 | // Some of the properties of this prim may have changed. |
@@ -518,13 +558,25 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
518 | { | 558 | { |
519 | if (IsActive) | 559 | if (IsActive) |
520 | { | 560 | { |
521 | // Friction effects are handled by this vehicle code | 561 | 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 | 562 | ||
527 | VDetailLog("{0},BSDynamics.Refresh,zeroingFriction and adding damping", Prim.LocalID); | 563 | // Friction effects are handled by this vehicle code |
564 | float friction = 0f; | ||
565 | BulletSimAPI.SetFriction2(Prim.PhysBody.ptr, friction); | ||
566 | |||
567 | // Moderate angular movement introduced by Bullet. | ||
568 | // TODO: possibly set AngularFactor and LinearFactor for the type of vehicle. | ||
569 | // Maybe compute linear and angular factor and damping from params. | ||
570 | float angularDamping = PhysicsScene.Params.vehicleAngularDamping; | ||
571 | BulletSimAPI.SetAngularDamping2(Prim.PhysBody.ptr, angularDamping); | ||
572 | |||
573 | // DEBUG DEBUG DEBUG: use uniform inertia to smooth movement added by Bullet | ||
574 | // Vector3 localInertia = new Vector3(1f, 1f, 1f); | ||
575 | Vector3 localInertia = new Vector3(m_vehicleMass, m_vehicleMass, m_vehicleMass); | ||
576 | BulletSimAPI.SetMassProps2(Prim.PhysBody.ptr, m_vehicleMass, localInertia); | ||
577 | |||
578 | VDetailLog("{0},BSDynamics.Refresh,frict={1},inert={2},aDamp={3}", | ||
579 | Prim.LocalID, friction, localInertia, angularDamping); | ||
528 | } | 580 | } |
529 | } | 581 | } |
530 | 582 | ||
@@ -551,111 +603,109 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
551 | { | 603 | { |
552 | if (!IsActive) return; | 604 | if (!IsActive) return; |
553 | 605 | ||
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); | 606 | MoveLinear(pTimestep); |
566 | // Commented out for debug | ||
567 | MoveAngular(pTimestep); | 607 | MoveAngular(pTimestep); |
568 | // Prim.ApplyTorqueImpulse(-Prim.RotationalVelocity * m_vehicleMass, false); // DEBUG DEBUG | ||
569 | // Prim.ForceRotationalVelocity = -Prim.RotationalVelocity; // DEBUG DEBUG | ||
570 | 608 | ||
571 | LimitRotation(pTimestep); | 609 | LimitRotation(pTimestep); |
572 | 610 | ||
573 | // remember the position so next step we can limit absolute movement effects | 611 | // remember the position so next step we can limit absolute movement effects |
574 | m_lastPositionVector = Prim.ForcePosition; | 612 | m_lastPositionVector = Prim.ForcePosition; |
575 | 613 | ||
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}", | 614 | VDetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}", |
584 | Prim.LocalID, Prim.ForcePosition, Prim.Force, Prim.ForceVelocity, Prim.RotationalVelocity); | 615 | Prim.LocalID, Prim.ForcePosition, Prim.Force, Prim.ForceVelocity, Prim.RotationalVelocity); |
585 | }// end Step | 616 | } |
586 | 617 | ||
587 | // Apply the effect of the linear motor. | 618 | // Apply the effect of the linear motor. |
588 | // Also does hover and float. | 619 | // Also does hover and float. |
589 | private void MoveLinear(float pTimestep) | 620 | private void MoveLinear(float pTimestep) |
590 | { | 621 | { |
591 | // m_linearMotorDirection is the target direction we are moving relative to the vehicle coordinates | 622 | Vector3 linearMotorContribution = m_linearMotor.Step(pTimestep); |
592 | // m_lastLinearVelocityVector is the current speed we are moving in that direction | 623 | |
593 | if (m_linearMotorDirection.LengthSquared() > 0.001f) | 624 | // Rotate new object velocity from vehicle relative to world coordinates |
594 | { | 625 | linearMotorContribution *= Prim.ForceOrientation; |
595 | Vector3 origDir = m_linearMotorDirection; // DEBUG | 626 | |
596 | Vector3 origVel = m_lastLinearVelocityVector; // DEBUG | 627 | // ================================================================== |
597 | // DEBUG: the vehicle velocity rotated to be relative to vehicle coordinates for comparison | 628 | // Gravity and Buoyancy |
598 | Vector3 vehicleVelocity = Prim.ForceVelocity * Quaternion.Inverse(Prim.ForceOrientation); // DEBUG | 629 | // There is some gravity, make a gravity force vector that is applied after object velocity. |
630 | // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; | ||
631 | Vector3 grav = Prim.PhysicsScene.DefaultGravity * (1f - m_VehicleBuoyancy); | ||
599 | 632 | ||
600 | // Add (desiredVelocity - lastAppliedVelocity) / howLongItShouldTakeToComplete | 633 | Vector3 pos = Prim.ForcePosition; |
601 | Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/(m_linearMotorTimescale) * pTimestep; | 634 | float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos); |
602 | m_lastLinearVelocityVector += addAmount; | ||
603 | 635 | ||
604 | float decayFactor = (1.0f / m_linearMotorDecayTimescale) * pTimestep; | 636 | Vector3 terrainHeightContribution = ComputeLinearTerrainHeightCorrection(pTimestep, ref pos, terrainHeight); |
605 | m_linearMotorDirection *= (1f - decayFactor); | ||
606 | 637 | ||
607 | // Rotate new object velocity from vehicle relative to world coordinates | 638 | Vector3 hoverContribution = ComputeLinearHover(pTimestep, ref pos, terrainHeight); |
608 | m_newVelocity = m_lastLinearVelocityVector * Prim.ForceOrientation; | ||
609 | 639 | ||
610 | // Apply friction for next time | 640 | ComputeLinearBlockingEndPoint(pTimestep, ref pos); |
611 | Vector3 frictionFactor = (Vector3.One / m_linearFrictionTimescale) * pTimestep; | ||
612 | m_lastLinearVelocityVector *= (Vector3.One - frictionFactor); | ||
613 | 641 | ||
614 | VDetailLog("{0},MoveLinear,nonZero,origlmDir={1},origlvVel={2},vehVel={3},add={4},decay={5},frict={6},lmDir={7},lvVec={8},newVel={9}", | 642 | Vector3 limitMotorUpContribution = ComputeLinearMotorUp(pTimestep, pos, terrainHeight); |
615 | Prim.LocalID, origDir, origVel, vehicleVelocity, addAmount, decayFactor, frictionFactor, | 643 | |
616 | m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity); | 644 | // ================================================================== |
617 | } | 645 | Vector3 newVelocity = linearMotorContribution |
618 | else | 646 | + terrainHeightContribution |
619 | { | 647 | + hoverContribution |
620 | // if what remains of direction is very small, zero it. | 648 | + limitMotorUpContribution; |
621 | m_linearMotorDirection = Vector3.Zero; | ||
622 | m_lastLinearVelocityVector = Vector3.Zero; | ||
623 | m_newVelocity = Vector3.Zero; | ||
624 | 649 | ||
625 | VDetailLog("{0},MoveLinear,zeroed", Prim.LocalID); | 650 | // If not changing some axis, reduce out velocity |
651 | if ((m_flags & (VehicleFlag.NO_X)) != 0) | ||
652 | newVelocity.X = 0; | ||
653 | if ((m_flags & (VehicleFlag.NO_Y)) != 0) | ||
654 | newVelocity.Y = 0; | ||
655 | if ((m_flags & (VehicleFlag.NO_Z)) != 0) | ||
656 | newVelocity.Z = 0; | ||
657 | |||
658 | // ================================================================== | ||
659 | // Clamp REALLY high or low velocities | ||
660 | float newVelocityLengthSq = newVelocity.LengthSquared(); | ||
661 | if (newVelocityLengthSq > 1e6f) | ||
662 | { | ||
663 | newVelocity /= newVelocity.Length(); | ||
664 | newVelocity *= 1000f; | ||
626 | } | 665 | } |
666 | else if (newVelocityLengthSq < 1e-6f) | ||
667 | newVelocity = Vector3.Zero; | ||
627 | 668 | ||
628 | // m_newVelocity is velocity computed from linear motor in world coordinates | 669 | // ================================================================== |
670 | // Stuff new linear velocity into the vehicle | ||
671 | Prim.ForceVelocity = newVelocity; | ||
672 | // Prim.ApplyForceImpulse((m_newVelocity - Prim.Velocity) * m_vehicleMass, false); // DEBUG DEBUG | ||
629 | 673 | ||
630 | // Gravity and Buoyancy | 674 | // Other linear forces are applied as forces. |
631 | // There is some gravity, make a gravity force vector that is applied after object velocity. | 675 | Vector3 totalDownForce = grav * m_vehicleMass; |
632 | // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; | 676 | if (totalDownForce != Vector3.Zero) |
633 | Vector3 grav = Prim.PhysicsScene.DefaultGravity * (1f - m_VehicleBuoyancy); | 677 | { |
678 | Prim.AddForce(totalDownForce, false); | ||
679 | } | ||
634 | 680 | ||
635 | /* | 681 | VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},primVel={4},totalDown={5}", |
636 | * RA: Not sure why one would do this unless we are hoping external forces are doing gravity, ... | 682 | Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector, |
637 | // Preserve the current Z velocity | 683 | newVelocity, Prim.Velocity, totalDownForce); |
638 | Vector3 vel_now = m_prim.Velocity; | ||
639 | m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity | ||
640 | */ | ||
641 | 684 | ||
642 | Vector3 pos = Prim.ForcePosition; | 685 | } // end MoveLinear() |
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 | 686 | ||
687 | public Vector3 ComputeLinearTerrainHeightCorrection(float pTimestep, ref Vector3 pos, float terrainHeight) | ||
688 | { | ||
689 | Vector3 ret = Vector3.Zero; | ||
645 | // If below the terrain, move us above the ground a little. | 690 | // If below the terrain, move us above the ground a little. |
646 | 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. | 691 | // Taking the rotated size doesn't work here because m_prim.Size is the size of the root prim and not the linkset. |
648 | // TODO: Add a m_prim.LinkSet.Size similar to m_prim.LinkSet.Mass. | 692 | // TODO: Add a m_prim.LinkSet.Size similar to m_prim.LinkSet.Mass. |
649 | // Vector3 rotatedSize = m_prim.Size * m_prim.ForceOrientation; | 693 | // Vector3 rotatedSize = m_prim.Size * m_prim.ForceOrientation; |
650 | // if (rotatedSize.Z < terrainHeight) | 694 | // if (rotatedSize.Z < terrainHeight) |
651 | if (pos.Z < terrainHeight) | 695 | if (pos.Z < terrainHeight) |
652 | { | 696 | { |
697 | // TODO: correct position by applying force rather than forcing position. | ||
653 | pos.Z = terrainHeight + 2; | 698 | pos.Z = terrainHeight + 2; |
654 | Prim.ForcePosition = pos; | 699 | Prim.ForcePosition = pos; |
655 | VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, terrainHeight, pos); | 700 | VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, terrainHeight, pos); |
656 | } | 701 | } |
702 | return ret; | ||
703 | } | ||
704 | |||
705 | public Vector3 ComputeLinearHover(float pTimestep, ref Vector3 pos, float terrainHeight) | ||
706 | { | ||
707 | Vector3 ret = Vector3.Zero; | ||
657 | 708 | ||
658 | // Check if hovering | ||
659 | // m_VhoverEfficiency: 0=bouncy, 1=totally damped | 709 | // m_VhoverEfficiency: 0=bouncy, 1=totally damped |
660 | // m_VhoverTimescale: time to achieve height | 710 | // m_VhoverTimescale: time to achieve height |
661 | if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) | 711 | if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) |
@@ -663,7 +713,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
663 | // We should hover, get the target height | 713 | // We should hover, get the target height |
664 | if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0) | 714 | if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0) |
665 | { | 715 | { |
666 | m_VhoverTargetHeight = Prim.PhysicsScene.GetWaterLevelAtXYZ(pos) + m_VhoverHeight; | 716 | m_VhoverTargetHeight = Prim.PhysicsScene.TerrainManager.GetWaterLevelAtXYZ(pos) + m_VhoverHeight; |
667 | } | 717 | } |
668 | if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) | 718 | if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) |
669 | { | 719 | { |
@@ -680,6 +730,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
680 | if (pos.Z > m_VhoverTargetHeight) | 730 | if (pos.Z > m_VhoverTargetHeight) |
681 | m_VhoverTargetHeight = pos.Z; | 731 | m_VhoverTargetHeight = pos.Z; |
682 | } | 732 | } |
733 | |||
683 | if ((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0) | 734 | if ((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0) |
684 | { | 735 | { |
685 | if (Math.Abs(pos.Z - m_VhoverTargetHeight) > 0.2f) | 736 | if (Math.Abs(pos.Z - m_VhoverTargetHeight) > 0.2f) |
@@ -694,28 +745,31 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
694 | // RA: where does the 50 come from? | 745 | // RA: where does the 50 come from? |
695 | float verticalCorrectionVelocity = pTimestep * ((verticalError * 50.0f) / m_VhoverTimescale); | 746 | float verticalCorrectionVelocity = pTimestep * ((verticalError * 50.0f) / m_VhoverTimescale); |
696 | // Replace Vertical speed with correction figure if significant | 747 | // Replace Vertical speed with correction figure if significant |
697 | if (Math.Abs(verticalError) > 0.01f) | 748 | if (verticalError > 0.01f) |
698 | { | 749 | { |
699 | m_newVelocity.Z += verticalCorrectionVelocity; | 750 | ret = new Vector3(0f, 0f, verticalCorrectionVelocity); |
700 | //KF: m_VhoverEfficiency is not yet implemented | 751 | //KF: m_VhoverEfficiency is not yet implemented |
701 | } | 752 | } |
702 | else if (verticalError < -0.01) | 753 | else if (verticalError < -0.01) |
703 | { | 754 | { |
704 | m_newVelocity.Z -= verticalCorrectionVelocity; | 755 | ret = new Vector3(0f, 0f, -verticalCorrectionVelocity); |
705 | } | ||
706 | else | ||
707 | { | ||
708 | m_newVelocity.Z = 0f; | ||
709 | } | 756 | } |
710 | } | 757 | } |
711 | 758 | ||
712 | VDetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}", Prim.LocalID, pos, m_newVelocity, m_VhoverHeight, m_VhoverTargetHeight); | 759 | VDetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}", |
760 | Prim.LocalID, pos, ret, m_VhoverHeight, m_VhoverTargetHeight); | ||
713 | } | 761 | } |
714 | 762 | ||
763 | return ret; | ||
764 | } | ||
765 | |||
766 | public bool ComputeLinearBlockingEndPoint(float pTimestep, ref Vector3 pos) | ||
767 | { | ||
768 | bool changed = false; | ||
769 | |||
715 | Vector3 posChange = pos - m_lastPositionVector; | 770 | Vector3 posChange = pos - m_lastPositionVector; |
716 | if (m_BlockingEndPoint != Vector3.Zero) | 771 | if (m_BlockingEndPoint != Vector3.Zero) |
717 | { | 772 | { |
718 | bool changed = false; | ||
719 | if (pos.X >= (m_BlockingEndPoint.X - (float)1)) | 773 | if (pos.X >= (m_BlockingEndPoint.X - (float)1)) |
720 | { | 774 | { |
721 | pos.X -= posChange.X + 1; | 775 | pos.X -= posChange.X + 1; |
@@ -748,75 +802,53 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
748 | Prim.LocalID, m_BlockingEndPoint, posChange, pos); | 802 | Prim.LocalID, m_BlockingEndPoint, posChange, pos); |
749 | } | 803 | } |
750 | } | 804 | } |
805 | return changed; | ||
806 | } | ||
751 | 807 | ||
752 | #region downForce | 808 | // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags : |
753 | Vector3 downForce = Vector3.Zero; | 809 | // Prevent ground vehicles from motoring into the sky.This flag has a subtle effect when |
754 | 810 | // used with conjunction with banking: the strength of the banking will decay when the | |
811 | // vehicle no longer experiences collisions. The decay timescale is the same as | ||
812 | // VEHICLE_BANKING_TIMESCALE. This is to help prevent ground vehicles from steering | ||
813 | // when they are in mid jump. | ||
814 | // TODO: this code is wrong. Also, what should it do for boats? | ||
815 | public Vector3 ComputeLinearMotorUp(float pTimestep, Vector3 pos, float terrainHeight) | ||
816 | { | ||
817 | Vector3 ret = Vector3.Zero; | ||
755 | if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0) | 818 | if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0) |
756 | { | 819 | { |
757 | // If the vehicle is motoring into the sky, get it going back down. | 820 | // 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; | 821 | float distanceAboveGround = pos.Z - terrainHeight; |
760 | if (distanceAboveGround > 2f) | 822 | if (distanceAboveGround > 1f) |
761 | { | 823 | { |
762 | // downForce = new Vector3(0, 0, (-distanceAboveGround / m_bankingTimescale) * pTimestep); | 824 | // downForce = new Vector3(0, 0, (-distanceAboveGround / m_bankingTimescale) * pTimestep); |
763 | // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale); | 825 | // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale); |
764 | downForce = new Vector3(0, 0, -distanceAboveGround); | 826 | ret = new Vector3(0, 0, -distanceAboveGround); |
765 | } | 827 | } |
766 | // TODO: this calculation is all wrong. From the description at | 828 | // TODO: this calculation is wrong. From the description at |
767 | // (http://wiki.secondlife.com/wiki/Category:LSL_Vehicle), the downForce | 829 | // (http://wiki.secondlife.com/wiki/Category:LSL_Vehicle), the downForce |
768 | // has a decay factor. This says this force should | 830 | // has a decay factor. This says this force should |
769 | // be computed with a motor. | 831 | // be computed with a motor. |
770 | VDetailLog("{0},MoveLinear,limitMotorUp,distAbove={1},downForce={2}", | 832 | // TODO: add interaction with banking. |
771 | Prim.LocalID, distanceAboveGround, downForce); | 833 | VDetailLog("{0},MoveLinear,limitMotorUp,distAbove={1},downForce={2}", |
772 | } | 834 | Prim.LocalID, distanceAboveGround, ret); |
773 | #endregion // downForce | ||
774 | |||
775 | // If not changing some axis, reduce out velocity | ||
776 | if ((m_flags & (VehicleFlag.NO_X)) != 0) | ||
777 | m_newVelocity.X = 0; | ||
778 | if ((m_flags & (VehicleFlag.NO_Y)) != 0) | ||
779 | m_newVelocity.Y = 0; | ||
780 | if ((m_flags & (VehicleFlag.NO_Z)) != 0) | ||
781 | m_newVelocity.Z = 0; | ||
782 | |||
783 | // Clamp REALLY high or low velocities | ||
784 | if (m_newVelocity.LengthSquared() > 1e6f) | ||
785 | { | ||
786 | m_newVelocity /= m_newVelocity.Length(); | ||
787 | m_newVelocity *= 1000f; | ||
788 | } | ||
789 | else if (m_newVelocity.LengthSquared() < 1e-6f) | ||
790 | m_newVelocity = Vector3.Zero; | ||
791 | |||
792 | // Stuff new linear velocity into the vehicle | ||
793 | Prim.ForceVelocity = m_newVelocity; | ||
794 | // Prim.ApplyForceImpulse((m_newVelocity - Prim.Velocity) * m_vehicleMass, false); // DEBUG DEBUG | ||
795 | |||
796 | Vector3 totalDownForce = downForce + grav; | ||
797 | if (totalDownForce != Vector3.Zero) | ||
798 | { | ||
799 | Prim.AddForce(totalDownForce * m_vehicleMass, false); | ||
800 | // Prim.ApplyForceImpulse(totalDownForce * m_vehicleMass, false); | ||
801 | } | 835 | } |
836 | return ret; | ||
837 | } | ||
802 | 838 | ||
803 | VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},primVel={4},totalDown={5}", | 839 | // ======================================================================= |
804 | Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity, Prim.Velocity, totalDownForce); | ||
805 | |||
806 | } // end MoveLinear() | ||
807 | |||
808 | // ======================================================================= | 840 | // ======================================================================= |
809 | // Apply the effect of the angular motor. | 841 | // Apply the effect of the angular motor. |
810 | private void MoveAngular(float pTimestep) | 842 | private void MoveAngular(float pTimestep) |
811 | { | 843 | { |
812 | // m_angularMotorDirection // angular velocity requested by LSL motor | 844 | // 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) | 845 | // m_angularMotorVelocity // current angular motor velocity (ramps up and down) |
815 | // m_angularMotorTimescale // motor angular velocity ramp up rate | 846 | // m_angularMotorTimescale // motor angular velocity ramp up time |
816 | // m_angularMotorDecayTimescale // motor angular velocity decay rate | 847 | // m_angularMotorDecayTimescale // motor angular velocity decay rate |
817 | // m_angularFrictionTimescale // body angular velocity decay rate | 848 | // m_angularFrictionTimescale // body angular velocity decay rate |
818 | // m_lastAngularVelocity // what was last applied to body | 849 | // m_lastAngularVelocity // what was last applied to body |
819 | 850 | ||
851 | /* | ||
820 | if (m_angularMotorDirection.LengthSquared() > 0.0001) | 852 | if (m_angularMotorDirection.LengthSquared() > 0.0001) |
821 | { | 853 | { |
822 | Vector3 origVel = m_angularMotorVelocity; | 854 | Vector3 origVel = m_angularMotorVelocity; |
@@ -835,59 +867,157 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
835 | { | 867 | { |
836 | m_angularMotorVelocity = Vector3.Zero; | 868 | m_angularMotorVelocity = Vector3.Zero; |
837 | } | 869 | } |
870 | */ | ||
838 | 871 | ||
839 | #region Vertical attactor | 872 | Vector3 angularMotorContribution = m_angularMotor.Step(pTimestep); |
840 | 873 | ||
841 | Vector3 vertattr = Vector3.Zero; | 874 | // ================================================================== |
842 | Vector3 deflection = Vector3.Zero; | 875 | // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags : |
843 | Vector3 banking = Vector3.Zero; | 876 | // This flag prevents linear deflection parallel to world z-axis. This is useful |
877 | // for preventing ground vehicles with large linear deflection, like bumper cars, | ||
878 | // from climbing their linear deflection into the sky. | ||
879 | // That is, NO_DEFLECTION_UP says angular motion should not add any pitch or roll movement | ||
880 | if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0) | ||
881 | { | ||
882 | angularMotorContribution.X = 0f; | ||
883 | angularMotorContribution.Y = 0f; | ||
884 | VDetailLog("{0},MoveAngular,noDeflectionUp,angularMotorContrib={1}", Prim.LocalID, angularMotorContribution); | ||
885 | } | ||
844 | 886 | ||
845 | // If vertical attaction timescale is reasonable and we applied an angular force last time... | 887 | Vector3 verticalAttractionContribution = ComputeAngularVerticalAttraction(pTimestep); |
846 | if (m_verticalAttractionTimescale < 300 && m_lastAngularVelocity != Vector3.Zero) | 888 | |
889 | Vector3 deflectionContribution = ComputeAngularDeflection(pTimestep); | ||
890 | |||
891 | Vector3 bankingContribution = ComputeAngularBanking(pTimestep); | ||
892 | |||
893 | // ================================================================== | ||
894 | m_lastVertAttractor = verticalAttractionContribution; | ||
895 | |||
896 | // Sum velocities | ||
897 | m_lastAngularVelocity = angularMotorContribution | ||
898 | + verticalAttractionContribution | ||
899 | + deflectionContribution | ||
900 | + bankingContribution; | ||
901 | |||
902 | // ================================================================== | ||
903 | //Offset section | ||
904 | if (m_linearMotorOffset != Vector3.Zero) | ||
905 | { | ||
906 | //Offset of linear velocity doesn't change the linear velocity, | ||
907 | // but causes a torque to be applied, for example... | ||
908 | // | ||
909 | // IIIII >>> IIIII | ||
910 | // IIIII >>> IIIII | ||
911 | // IIIII >>> IIIII | ||
912 | // ^ | ||
913 | // | Applying a force at the arrow will cause the object to move forward, but also rotate | ||
914 | // | ||
915 | // | ||
916 | // The torque created is the linear velocity crossed with the offset | ||
917 | |||
918 | // TODO: this computation should be in the linear section | ||
919 | // because that is where we know the impulse being applied. | ||
920 | Vector3 torqueFromOffset = Vector3.Zero; | ||
921 | // torqueFromOffset = Vector3.Cross(m_linearMotorOffset, appliedImpulse); | ||
922 | if (float.IsNaN(torqueFromOffset.X)) | ||
923 | torqueFromOffset.X = 0; | ||
924 | if (float.IsNaN(torqueFromOffset.Y)) | ||
925 | torqueFromOffset.Y = 0; | ||
926 | if (float.IsNaN(torqueFromOffset.Z)) | ||
927 | torqueFromOffset.Z = 0; | ||
928 | torqueFromOffset *= m_vehicleMass; | ||
929 | Prim.ApplyTorqueImpulse(torqueFromOffset, true); | ||
930 | VDetailLog("{0},BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset); | ||
931 | } | ||
932 | |||
933 | // ================================================================== | ||
934 | if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f)) | ||
935 | { | ||
936 | m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero. | ||
937 | // TODO: zeroing is good but it also sets values in unmanaged code. Remove the stores when idle. | ||
938 | VDetailLog("{0},MoveAngular,zeroAngularMotion,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity); | ||
939 | Prim.ZeroAngularMotion(true); | ||
940 | } | ||
941 | else | ||
847 | { | 942 | { |
848 | float VAservo = pTimestep * 0.2f / m_verticalAttractionTimescale; | 943 | // Apply to the body. |
849 | if (Prim.IsColliding) | 944 | // The above calculates the absolute angular velocity needed. Angular velocity is massless. |
850 | VAservo = pTimestep * 0.05f / (m_verticalAttractionTimescale); | 945 | // Since we are stuffing the angular velocity directly into the object, the computed |
946 | // velocity needs to be scaled by the timestep. | ||
947 | // Also remove any motion that is on the object so added motion is only from vehicle. | ||
948 | Vector3 applyAngularForce = ((m_lastAngularVelocity * pTimestep) | ||
949 | - Prim.ForceRotationalVelocity); | ||
950 | // Unscale the force by the angular factor so it overwhelmes the Bullet additions. | ||
951 | Prim.ForceRotationalVelocity = applyAngularForce; | ||
952 | |||
953 | VDetailLog("{0},MoveAngular,done,angMotor={1},vertAttr={2},bank={3},deflect={4},newAngForce={5},lastAngular={6}", | ||
954 | Prim.LocalID, | ||
955 | angularMotorContribution, verticalAttractionContribution, | ||
956 | bankingContribution, deflectionContribution, | ||
957 | applyAngularForce, m_lastAngularVelocity | ||
958 | ); | ||
959 | } | ||
960 | } | ||
851 | 961 | ||
852 | VAservo *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency); | 962 | public Vector3 ComputeAngularVerticalAttraction(float pTimestep) |
963 | { | ||
964 | Vector3 ret = Vector3.Zero; | ||
853 | 965 | ||
854 | // Create a vector of the vehicle "up" in world coordinates | 966 | // If vertical attaction timescale is reasonable and we applied an angular force last time... |
967 | if (m_verticalAttractionTimescale < 500) | ||
968 | { | ||
969 | Vector3 verticalError = Vector3.UnitZ * Prim.ForceOrientation; | ||
970 | verticalError.Normalize(); | ||
971 | m_verticalAttractionMotor.SetCurrent(verticalError); | ||
972 | m_verticalAttractionMotor.SetTarget(Vector3.UnitZ); | ||
973 | ret = m_verticalAttractionMotor.Step(pTimestep); | ||
974 | /* | ||
975 | // Take a vector pointing up and convert it from world to vehicle relative coords. | ||
855 | Vector3 verticalError = Vector3.UnitZ * Prim.ForceOrientation; | 976 | Vector3 verticalError = Vector3.UnitZ * Prim.ForceOrientation; |
856 | // verticalError.X and .Y are the World error amounts. They are 0 when there is no | 977 | verticalError.Normalize(); |
857 | // error (Vehicle Body is 'vertical'), and .Z will be 1. As the body leans to its | 978 | |
858 | // side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall | 979 | // If vertical attraction correction is needed, the vector that was pointing up (UnitZ) |
859 | // and .Z will go // negative. Similar for tilt and |.Y|. .X and .Y must be | 980 | // is now leaning to one side (rotated around the X axis) and the Y value will |
860 | // modulated to prevent a stable inverted body. | 981 | // go from zero (nearly straight up) to one (completely to the side) or leaning |
861 | 982 | // front-to-back (rotated around the Y axis) and the value of X will be between | |
862 | // Error is 0 (no error) to +/- 2 (max error) | 983 | // zero and one. |
863 | if (verticalError.Z < 0.0f) | 984 | // The value of Z is how far the rotation is off with 1 meaning none and 0 being 90 degrees. |
985 | |||
986 | // If verticalError.Z is negative, the vehicle is upside down. Add additional push. | ||
987 | if (verticalError.Z < 0f) | ||
864 | { | 988 | { |
865 | verticalError.X = 2.0f - verticalError.X; | 989 | verticalError.X = 2f - verticalError.X; |
866 | verticalError.Y = 2.0f - verticalError.Y; | 990 | verticalError.Y = 2f - verticalError.Y; |
867 | } | 991 | } |
868 | // scale it by VAservo (timestep and timescale) | ||
869 | verticalError = verticalError * VAservo; | ||
870 | 992 | ||
871 | // As the body rotates around the X axis, then verticalError.Y increases; Rotated around Y | 993 | // Y error means needed rotation around X axis and visa versa. |
872 | // then .X increases, so change Body angular velocity X based on Y, and Y based on X. | 994 | verticalAttractionContribution.X = verticalError.Y; |
873 | // Z is not changed. | 995 | verticalAttractionContribution.Y = - verticalError.X; |
874 | vertattr.X = verticalError.Y; | 996 | verticalAttractionContribution.Z = 0f; |
875 | vertattr.Y = - verticalError.X; | ||
876 | vertattr.Z = 0f; | ||
877 | 997 | ||
878 | // scaling appears better usingsquare-law | 998 | // scale by the time scale and timestep |
879 | Vector3 angularVelocity = Prim.ForceRotationalVelocity; | 999 | Vector3 unscaledContrib = verticalAttractionContribution; |
880 | float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency); | 1000 | verticalAttractionContribution /= m_verticalAttractionTimescale; |
881 | vertattr.X += bounce * angularVelocity.X; | 1001 | verticalAttractionContribution *= pTimestep; |
882 | vertattr.Y += bounce * angularVelocity.Y; | ||
883 | 1002 | ||
884 | VDetailLog("{0},MoveAngular,verticalAttraction,VAservo={1},effic={2},verticalError={3},bounce={4},vertattr={5}", | 1003 | // apply efficiency |
885 | Prim.LocalID, VAservo, m_verticalAttractionEfficiency, verticalError, bounce, vertattr); | 1004 | Vector3 preEfficiencyContrib = verticalAttractionContribution; |
1005 | float efficencySquared = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency; | ||
1006 | verticalAttractionContribution *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency); | ||
1007 | |||
1008 | VDetailLog("{0},MoveAngular,verticalAttraction,,verticalError={1},unscaled={2},preEff={3},eff={4},effSq={5},vertAttr={6}", | ||
1009 | Prim.LocalID, verticalError, unscaledContrib, preEfficiencyContrib, | ||
1010 | m_verticalAttractionEfficiency, efficencySquared, | ||
1011 | verticalAttractionContribution); | ||
1012 | */ | ||
886 | 1013 | ||
887 | } | 1014 | } |
888 | #endregion // Vertical attactor | 1015 | return ret; |
1016 | } | ||
889 | 1017 | ||
890 | #region Deflection | 1018 | public Vector3 ComputeAngularDeflection(float pTimestep) |
1019 | { | ||
1020 | Vector3 ret = Vector3.Zero; | ||
891 | 1021 | ||
892 | if (m_angularDeflectionEfficiency != 0) | 1022 | if (m_angularDeflectionEfficiency != 0) |
893 | { | 1023 | { |
@@ -899,32 +1029,35 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
899 | Vector3 preferredAxisOfMotion = scaledDefaultDirection * Quaternion.Add(Prim.ForceOrientation, m_referenceFrame); | 1029 | Vector3 preferredAxisOfMotion = scaledDefaultDirection * Quaternion.Add(Prim.ForceOrientation, m_referenceFrame); |
900 | 1030 | ||
901 | // Scale by efficiency and timescale | 1031 | // Scale by efficiency and timescale |
902 | deflection = (preferredAxisOfMotion * (m_angularDeflectionEfficiency) / m_angularDeflectionTimescale) * pTimestep; | 1032 | ret = (preferredAxisOfMotion * (m_angularDeflectionEfficiency) / m_angularDeflectionTimescale) * pTimestep; |
1033 | |||
1034 | VDetailLog("{0},MoveAngular,Deflection,perfAxis={1},deflection={2}", Prim.LocalID, preferredAxisOfMotion, ret); | ||
903 | 1035 | ||
904 | VDetailLog("{0},MoveAngular,Deflection,perfAxis={1},deflection={2}", | ||
905 | Prim.LocalID, preferredAxisOfMotion, deflection); | ||
906 | // This deflection computation is not correct. | 1036 | // This deflection computation is not correct. |
907 | deflection = Vector3.Zero; | 1037 | ret = Vector3.Zero; |
908 | } | 1038 | } |
1039 | return ret; | ||
1040 | } | ||
909 | 1041 | ||
910 | #endregion | 1042 | public Vector3 ComputeAngularBanking(float pTimestep) |
911 | 1043 | { | |
912 | #region Banking | 1044 | Vector3 ret = Vector3.Zero; |
913 | 1045 | ||
914 | if (m_bankingEfficiency != 0) | 1046 | if (m_bankingEfficiency != 0) |
915 | { | 1047 | { |
916 | Vector3 dir = Vector3.One * Prim.ForceOrientation; | 1048 | Vector3 dir = Vector3.One * Prim.ForceOrientation; |
917 | float mult = (m_bankingMix*m_bankingMix)*-1*(m_bankingMix < 0 ? -1 : 1); | 1049 | float mult = (m_bankingMix * m_bankingMix) * -1 * (m_bankingMix < 0 ? -1 : 1); |
918 | //Changes which way it banks in and out of turns | 1050 | //Changes which way it banks in and out of turns |
919 | 1051 | ||
920 | //Use the square of the efficiency, as it looks much more how SL banking works | 1052 | //Use the square of the efficiency, as it looks much more how SL banking works |
921 | float effSquared = (m_bankingEfficiency*m_bankingEfficiency); | 1053 | float effSquared = (m_bankingEfficiency * m_bankingEfficiency); |
922 | if (m_bankingEfficiency < 0) | 1054 | if (m_bankingEfficiency < 0) |
923 | effSquared *= -1; //Keep the negative! | 1055 | effSquared *= -1; //Keep the negative! |
924 | 1056 | ||
925 | float mix = Math.Abs(m_bankingMix); | 1057 | float mix = Math.Abs(m_bankingMix); |
926 | if (m_angularMotorVelocity.X == 0) | 1058 | if (m_angularMotorVelocity.X == 0) |
927 | { | 1059 | { |
1060 | // The vehicle is stopped | ||
928 | /*if (!parent.Orientation.ApproxEquals(this.m_referenceFrame, 0.25f)) | 1061 | /*if (!parent.Orientation.ApproxEquals(this.m_referenceFrame, 0.25f)) |
929 | { | 1062 | { |
930 | Vector3 axisAngle; | 1063 | Vector3 axisAngle; |
@@ -938,101 +1071,39 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
938 | }*/ | 1071 | }*/ |
939 | } | 1072 | } |
940 | else | 1073 | else |
941 | banking.Z += (effSquared*(mult*mix))*(m_angularMotorVelocity.X) * 4; | 1074 | { |
1075 | ret.Z += (effSquared * (mult * mix)) * (m_angularMotorVelocity.X) * 4; | ||
1076 | } | ||
1077 | |||
1078 | //If they are colliding, we probably shouldn't shove the prim around... probably | ||
942 | if (!Prim.IsColliding && Math.Abs(m_angularMotorVelocity.X) > mix) | 1079 | if (!Prim.IsColliding && Math.Abs(m_angularMotorVelocity.X) > mix) |
943 | //If they are colliding, we probably shouldn't shove the prim around... probably | ||
944 | { | 1080 | { |
945 | float angVelZ = m_angularMotorVelocity.X*-1; | 1081 | float angVelZ = m_angularMotorVelocity.X * -1; |
946 | /*if(angVelZ > mix) | 1082 | /*if(angVelZ > mix) |
947 | angVelZ = mix; | 1083 | angVelZ = mix; |
948 | else if(angVelZ < -mix) | 1084 | else if(angVelZ < -mix) |
949 | angVelZ = -mix;*/ | 1085 | angVelZ = -mix;*/ |
950 | //This controls how fast and how far the banking occurs | 1086 | //This controls how fast and how far the banking occurs |
951 | Vector3 bankingRot = new Vector3(angVelZ*(effSquared*mult), 0, 0); | 1087 | Vector3 bankingRot = new Vector3(angVelZ * (effSquared * mult), 0, 0); |
952 | if (bankingRot.X > 3) | 1088 | if (bankingRot.X > 3) |
953 | bankingRot.X = 3; | 1089 | bankingRot.X = 3; |
954 | else if (bankingRot.X < -3) | 1090 | else if (bankingRot.X < -3) |
955 | bankingRot.X = -3; | 1091 | bankingRot.X = -3; |
956 | bankingRot *= Prim.ForceOrientation; | 1092 | bankingRot *= Prim.ForceOrientation; |
957 | banking += bankingRot; | 1093 | ret += bankingRot; |
958 | } | 1094 | } |
959 | m_angularMotorVelocity.X *= m_bankingEfficiency == 1 ? 0.0f : 1 - m_bankingEfficiency; | 1095 | m_angularMotorVelocity.X *= m_bankingEfficiency == 1 ? 0.0f : 1 - m_bankingEfficiency; |
960 | VDetailLog("{0},MoveAngular,Banking,bEff={1},angMotVel={2},banking={3}", | 1096 | VDetailLog("{0},MoveAngular,Banking,bEff={1},angMotVel={2},effSq={3},mult={4},mix={5},banking={6}", |
961 | Prim.LocalID, m_bankingEfficiency, m_angularMotorVelocity, banking); | 1097 | Prim.LocalID, m_bankingEfficiency, m_angularMotorVelocity, effSquared, mult, mix, ret); |
962 | } | ||
963 | |||
964 | #endregion | ||
965 | |||
966 | m_lastVertAttractor = vertattr; | ||
967 | |||
968 | // Sum velocities | ||
969 | m_lastAngularVelocity = m_angularMotorVelocity + vertattr + banking + deflection; | ||
970 | |||
971 | #region Linear Motor Offset | ||
972 | |||
973 | //Offset section | ||
974 | if (m_linearMotorOffset != Vector3.Zero) | ||
975 | { | ||
976 | //Offset of linear velocity doesn't change the linear velocity, | ||
977 | // but causes a torque to be applied, for example... | ||
978 | // | ||
979 | // IIIII >>> IIIII | ||
980 | // IIIII >>> IIIII | ||
981 | // IIIII >>> IIIII | ||
982 | // ^ | ||
983 | // | Applying a force at the arrow will cause the object to move forward, but also rotate | ||
984 | // | ||
985 | // | ||
986 | // The torque created is the linear velocity crossed with the offset | ||
987 | |||
988 | // NOTE: this computation does should be in the linear section | ||
989 | // because there we know the impulse being applied. | ||
990 | Vector3 torqueFromOffset = Vector3.Zero; | ||
991 | // torqueFromOffset = Vector3.Cross(m_linearMotorOffset, appliedImpulse); | ||
992 | if (float.IsNaN(torqueFromOffset.X)) | ||
993 | torqueFromOffset.X = 0; | ||
994 | if (float.IsNaN(torqueFromOffset.Y)) | ||
995 | torqueFromOffset.Y = 0; | ||
996 | if (float.IsNaN(torqueFromOffset.Z)) | ||
997 | torqueFromOffset.Z = 0; | ||
998 | torqueFromOffset *= m_vehicleMass; | ||
999 | Prim.ApplyTorqueImpulse(torqueFromOffset, true); | ||
1000 | VDetailLog("{0},BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset); | ||
1001 | } | ||
1002 | |||
1003 | #endregion | ||
1004 | |||
1005 | if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0) | ||
1006 | { | ||
1007 | m_lastAngularVelocity.X = 0; | ||
1008 | m_lastAngularVelocity.Y = 0; | ||
1009 | VDetailLog("{0},MoveAngular,noDeflectionUp,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity); | ||
1010 | } | 1098 | } |
1099 | return ret; | ||
1100 | } | ||
1011 | 1101 | ||
1012 | if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f)) | ||
1013 | { | ||
1014 | m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero. | ||
1015 | Prim.ZeroAngularMotion(true); | ||
1016 | VDetailLog("{0},MoveAngular,zeroAngularMotion,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity); | ||
1017 | } | ||
1018 | else | ||
1019 | { | ||
1020 | // Apply to the body. | ||
1021 | // 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 | ||
1023 | // velocity needs to be scaled by the timestep. | ||
1024 | Vector3 applyAngularForce = ((m_lastAngularVelocity * pTimestep) - Prim.ForceRotationalVelocity); | ||
1025 | Prim.ForceRotationalVelocity = applyAngularForce; | ||
1026 | |||
1027 | // Decay the angular movement for next time | ||
1028 | Vector3 decayamount = (Vector3.One / m_angularFrictionTimescale) * pTimestep; | ||
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 | } | ||
1034 | } //end MoveAngular | ||
1035 | 1102 | ||
1103 | // This is from previous instantiations of XXXDynamics.cs. | ||
1104 | // Applies roll reference frame. | ||
1105 | // TODO: is this the right way to separate the code to do this operation? | ||
1106 | // Should this be in MoveAngular()? | ||
1036 | internal void LimitRotation(float timestep) | 1107 | internal void LimitRotation(float timestep) |
1037 | { | 1108 | { |
1038 | Quaternion rotq = Prim.ForceOrientation; | 1109 | Quaternion rotq = Prim.ForceOrientation; |