diff options
Diffstat (limited to '')
-rw-r--r-- | OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs | 1216 |
1 files changed, 827 insertions, 389 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs index dbc9039..235cefc 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs | |||
@@ -24,28 +24,16 @@ | |||
24 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS | 24 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS |
25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. | 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
26 | * | 26 | * |
27 | 27 | * The quotations from http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial | |
28 | /* RA: June 14, 2011. Copied from ODEDynamics.cs and converted to | 28 | * are Copyright (c) 2009 Linden Research, Inc and are used under their license |
29 | * call the BulletSim system. | 29 | * of Creative Commons Attribution-Share Alike 3.0 |
30 | */ | 30 | * (http://creativecommons.org/licenses/by-sa/3.0/). |
31 | /* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces | ||
32 | * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: | ||
33 | * ODEPrim.cs contains methods dealing with Prim editing, Prim | ||
34 | * characteristics and Kinetic motion. | ||
35 | * ODEDynamics.cs contains methods dealing with Prim Physical motion | ||
36 | * (dynamics) and the associated settings. Old Linear and angular | ||
37 | * motors for dynamic motion have been replace with MoveLinear() | ||
38 | * and MoveAngular(); 'Physical' is used only to switch ODE dynamic | ||
39 | * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to | ||
40 | * switch between 'VEHICLE' parameter use and general dynamics | ||
41 | * settings use. | ||
42 | */ | 31 | */ |
43 | 32 | ||
44 | using System; | 33 | using System; |
45 | using System.Collections.Generic; | 34 | using System.Collections.Generic; |
46 | using System.Reflection; | 35 | using System.Reflection; |
47 | using System.Runtime.InteropServices; | 36 | using System.Runtime.InteropServices; |
48 | using log4net; | ||
49 | using OpenMetaverse; | 37 | using OpenMetaverse; |
50 | using OpenSim.Framework; | 38 | using OpenSim.Framework; |
51 | using OpenSim.Region.Physics.Manager; | 39 | using OpenSim.Region.Physics.Manager; |
@@ -80,10 +68,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
80 | private Quaternion m_referenceFrame = Quaternion.Identity; | 68 | private Quaternion m_referenceFrame = Quaternion.Identity; |
81 | 69 | ||
82 | // Linear properties | 70 | // Linear properties |
71 | private BSVMotor m_linearMotor = new BSVMotor("LinearMotor"); | ||
83 | private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time | 72 | 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 | 73 | 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 | 74 | 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; | 75 | private Vector3 m_linearFrictionTimescale = Vector3.Zero; |
88 | private float m_linearMotorDecayTimescale = 0; | 76 | private float m_linearMotorDecayTimescale = 0; |
89 | private float m_linearMotorTimescale = 0; | 77 | private float m_linearMotorTimescale = 0; |
@@ -93,16 +81,18 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
93 | // private Vector3 m_linearMotorOffset = Vector3.Zero; | 81 | // private Vector3 m_linearMotorOffset = Vector3.Zero; |
94 | 82 | ||
95 | //Angular properties | 83 | //Angular properties |
84 | private BSVMotor m_angularMotor = new BSVMotor("AngularMotor"); | ||
96 | private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor | 85 | private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor |
97 | // private int m_angularMotorApply = 0; // application frame counter | 86 | // private int m_angularMotorApply = 0; // application frame counter |
98 | private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity | 87 | private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity |
99 | private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate | 88 | private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate |
100 | private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate | 89 | private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate |
101 | private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate | 90 | private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate |
102 | private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body | 91 | private Vector3 m_lastAngularVelocity = Vector3.Zero; |
103 | private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body | 92 | private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body |
104 | 93 | ||
105 | //Deflection properties | 94 | //Deflection properties |
95 | private BSVMotor m_angularDeflectionMotor = new BSVMotor("AngularDeflection"); | ||
106 | private float m_angularDeflectionEfficiency = 0; | 96 | private float m_angularDeflectionEfficiency = 0; |
107 | private float m_angularDeflectionTimescale = 0; | 97 | private float m_angularDeflectionTimescale = 0; |
108 | private float m_linearDeflectionEfficiency = 0; | 98 | private float m_linearDeflectionEfficiency = 0; |
@@ -114,33 +104,68 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
114 | private float m_bankingTimescale = 0; | 104 | private float m_bankingTimescale = 0; |
115 | 105 | ||
116 | //Hover and Buoyancy properties | 106 | //Hover and Buoyancy properties |
107 | private BSVMotor m_hoverMotor = new BSVMotor("Hover"); | ||
117 | private float m_VhoverHeight = 0f; | 108 | private float m_VhoverHeight = 0f; |
118 | private float m_VhoverEfficiency = 0f; | 109 | private float m_VhoverEfficiency = 0f; |
119 | private float m_VhoverTimescale = 0f; | 110 | private float m_VhoverTimescale = 0f; |
120 | private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height | 111 | private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height |
121 | private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle. | 112 | // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) |
122 | // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) | 113 | private float m_VehicleBuoyancy = 0f; |
123 | // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. | 114 | private Vector3 m_VehicleGravity = Vector3.Zero; // Gravity computed when buoyancy set |
124 | // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. | ||
125 | 115 | ||
126 | //Attractor properties | 116 | //Attractor properties |
127 | private float m_verticalAttractionEfficiency = 1.0f; // damped | 117 | private BSVMotor m_verticalAttractionMotor = new BSVMotor("VerticalAttraction"); |
128 | private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor. | 118 | private float m_verticalAttractionEfficiency = 1.0f; // damped |
119 | private float m_verticalAttractionCutoff = 500f; // per the documentation | ||
120 | // Timescale > cutoff means no vert attractor. | ||
121 | private float m_verticalAttractionTimescale = 510f; | ||
122 | |||
123 | // Just some recomputed constants: | ||
124 | static readonly float PIOverFour = ((float)Math.PI) / 4f; | ||
125 | static readonly float PIOverTwo = ((float)Math.PI) / 2f; | ||
126 | |||
127 | // For debugging, flags to turn on and off individual corrections. | ||
128 | public bool enableAngularVerticalAttraction; | ||
129 | public bool enableAngularDeflection; | ||
130 | public bool enableAngularBanking; | ||
129 | 131 | ||
130 | public BSDynamics(BSScene myScene, BSPrim myPrim) | 132 | public BSDynamics(BSScene myScene, BSPrim myPrim) |
131 | { | 133 | { |
132 | PhysicsScene = myScene; | 134 | PhysicsScene = myScene; |
133 | Prim = myPrim; | 135 | Prim = myPrim; |
134 | Type = Vehicle.TYPE_NONE; | 136 | Type = Vehicle.TYPE_NONE; |
137 | SetupVehicleDebugging(); | ||
138 | } | ||
139 | |||
140 | // Stopgap debugging enablement. Allows source level debugging but still checking | ||
141 | // in changes by making enablement of debugging flags from INI file. | ||
142 | public void SetupVehicleDebugging() | ||
143 | { | ||
144 | enableAngularVerticalAttraction = true; | ||
145 | enableAngularDeflection = false; | ||
146 | enableAngularBanking = false; | ||
147 | if (BSParam.VehicleDebuggingEnabled) | ||
148 | { | ||
149 | enableAngularVerticalAttraction = true; | ||
150 | enableAngularDeflection = false; | ||
151 | enableAngularBanking = false; | ||
152 | } | ||
135 | } | 153 | } |
136 | 154 | ||
137 | // Return 'true' if this vehicle is doing vehicle things | 155 | // Return 'true' if this vehicle is doing vehicle things |
138 | public bool IsActive | 156 | public bool IsActive |
139 | { | 157 | { |
140 | get { return Type != Vehicle.TYPE_NONE; } | 158 | get { return (Type != Vehicle.TYPE_NONE && Prim.IsPhysicallyActive); } |
141 | } | 159 | } |
142 | 160 | ||
143 | internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) | 161 | // Return 'true' if this a vehicle that should be sitting on the ground |
162 | public bool IsGroundVehicle | ||
163 | { | ||
164 | get { return (Type == Vehicle.TYPE_CAR || Type == Vehicle.TYPE_SLED); } | ||
165 | } | ||
166 | |||
167 | #region Vehicle parameter setting | ||
168 | public void ProcessFloatVehicleParam(Vehicle pParam, float pValue) | ||
144 | { | 169 | { |
145 | VDetailLog("{0},ProcessFloatVehicleParam,param={1},val={2}", Prim.LocalID, pParam, pValue); | 170 | VDetailLog("{0},ProcessFloatVehicleParam,param={1},val={2}", Prim.LocalID, pParam, pValue); |
146 | switch (pParam) | 171 | switch (pParam) |
@@ -152,13 +177,15 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
152 | m_angularDeflectionTimescale = Math.Max(pValue, 0.01f); | 177 | m_angularDeflectionTimescale = Math.Max(pValue, 0.01f); |
153 | break; | 178 | break; |
154 | case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: | 179 | case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: |
155 | m_angularMotorDecayTimescale = Math.Max(pValue, 0.01f); | 180 | m_angularMotorDecayTimescale = ClampInRange(0.01f, pValue, 120); |
181 | m_angularMotor.TargetValueDecayTimeScale = m_angularMotorDecayTimescale; | ||
156 | break; | 182 | break; |
157 | case Vehicle.ANGULAR_MOTOR_TIMESCALE: | 183 | case Vehicle.ANGULAR_MOTOR_TIMESCALE: |
158 | m_angularMotorTimescale = Math.Max(pValue, 0.01f); | 184 | m_angularMotorTimescale = Math.Max(pValue, 0.01f); |
185 | m_angularMotor.TimeScale = m_angularMotorTimescale; | ||
159 | break; | 186 | break; |
160 | case Vehicle.BANKING_EFFICIENCY: | 187 | case Vehicle.BANKING_EFFICIENCY: |
161 | m_bankingEfficiency = Math.Max(-1f, Math.Min(pValue, 1f)); | 188 | m_bankingEfficiency = ClampInRange(-1f, pValue, 1f); |
162 | break; | 189 | break; |
163 | case Vehicle.BANKING_MIX: | 190 | case Vehicle.BANKING_MIX: |
164 | m_bankingMix = Math.Max(pValue, 0.01f); | 191 | m_bankingMix = Math.Max(pValue, 0.01f); |
@@ -167,10 +194,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
167 | m_bankingTimescale = Math.Max(pValue, 0.01f); | 194 | m_bankingTimescale = Math.Max(pValue, 0.01f); |
168 | break; | 195 | break; |
169 | case Vehicle.BUOYANCY: | 196 | case Vehicle.BUOYANCY: |
170 | m_VehicleBuoyancy = Math.Max(-1f, Math.Min(pValue, 1f)); | 197 | m_VehicleBuoyancy = ClampInRange(-1f, pValue, 1f); |
198 | m_VehicleGravity = Prim.ComputeGravity(m_VehicleBuoyancy); | ||
171 | break; | 199 | break; |
172 | case Vehicle.HOVER_EFFICIENCY: | 200 | case Vehicle.HOVER_EFFICIENCY: |
173 | m_VhoverEfficiency = Math.Max(0f, Math.Min(pValue, 1f)); | 201 | m_VhoverEfficiency = ClampInRange(0f, pValue, 1f); |
174 | break; | 202 | break; |
175 | case Vehicle.HOVER_HEIGHT: | 203 | case Vehicle.HOVER_HEIGHT: |
176 | m_VhoverHeight = pValue; | 204 | m_VhoverHeight = pValue; |
@@ -185,33 +213,41 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
185 | m_linearDeflectionTimescale = Math.Max(pValue, 0.01f); | 213 | m_linearDeflectionTimescale = Math.Max(pValue, 0.01f); |
186 | break; | 214 | break; |
187 | case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: | 215 | case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: |
188 | m_linearMotorDecayTimescale = Math.Max(pValue, 0.01f); | 216 | m_linearMotorDecayTimescale = ClampInRange(0.01f, pValue, 120); |
217 | m_linearMotor.TargetValueDecayTimeScale = m_linearMotorDecayTimescale; | ||
189 | break; | 218 | break; |
190 | case Vehicle.LINEAR_MOTOR_TIMESCALE: | 219 | case Vehicle.LINEAR_MOTOR_TIMESCALE: |
191 | m_linearMotorTimescale = Math.Max(pValue, 0.01f); | 220 | m_linearMotorTimescale = Math.Max(pValue, 0.01f); |
221 | m_linearMotor.TimeScale = m_linearMotorTimescale; | ||
192 | break; | 222 | break; |
193 | case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: | 223 | case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: |
194 | m_verticalAttractionEfficiency = Math.Max(0.1f, Math.Min(pValue, 1f)); | 224 | m_verticalAttractionEfficiency = ClampInRange(0.1f, pValue, 1f); |
225 | m_verticalAttractionMotor.Efficiency = m_verticalAttractionEfficiency; | ||
195 | break; | 226 | break; |
196 | case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: | 227 | case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: |
197 | m_verticalAttractionTimescale = Math.Max(pValue, 0.01f); | 228 | m_verticalAttractionTimescale = Math.Max(pValue, 0.01f); |
229 | m_verticalAttractionMotor.TimeScale = m_verticalAttractionTimescale; | ||
198 | break; | 230 | break; |
199 | 231 | ||
200 | // These are vector properties but the engine lets you use a single float value to | 232 | // 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 | 233 | // set all of the components to the same value |
202 | case Vehicle.ANGULAR_FRICTION_TIMESCALE: | 234 | case Vehicle.ANGULAR_FRICTION_TIMESCALE: |
203 | m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); | 235 | m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); |
236 | m_angularMotor.FrictionTimescale = m_angularFrictionTimescale; | ||
204 | break; | 237 | break; |
205 | case Vehicle.ANGULAR_MOTOR_DIRECTION: | 238 | case Vehicle.ANGULAR_MOTOR_DIRECTION: |
206 | m_angularMotorDirection = new Vector3(pValue, pValue, pValue); | 239 | m_angularMotorDirection = new Vector3(pValue, pValue, pValue); |
207 | // m_angularMotorApply = 100; | 240 | m_angularMotor.Zero(); |
241 | m_angularMotor.SetTarget(m_angularMotorDirection); | ||
208 | break; | 242 | break; |
209 | case Vehicle.LINEAR_FRICTION_TIMESCALE: | 243 | case Vehicle.LINEAR_FRICTION_TIMESCALE: |
210 | m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); | 244 | m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); |
245 | m_linearMotor.FrictionTimescale = m_linearFrictionTimescale; | ||
211 | break; | 246 | break; |
212 | case Vehicle.LINEAR_MOTOR_DIRECTION: | 247 | case Vehicle.LINEAR_MOTOR_DIRECTION: |
213 | m_linearMotorDirection = new Vector3(pValue, pValue, pValue); | 248 | m_linearMotorDirection = new Vector3(pValue, pValue, pValue); |
214 | m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue); | 249 | m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue); |
250 | m_linearMotor.SetTarget(m_linearMotorDirection); | ||
215 | break; | 251 | break; |
216 | case Vehicle.LINEAR_MOTOR_OFFSET: | 252 | case Vehicle.LINEAR_MOTOR_OFFSET: |
217 | m_linearMotorOffset = new Vector3(pValue, pValue, pValue); | 253 | m_linearMotorOffset = new Vector3(pValue, pValue, pValue); |
@@ -227,21 +263,25 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
227 | { | 263 | { |
228 | case Vehicle.ANGULAR_FRICTION_TIMESCALE: | 264 | case Vehicle.ANGULAR_FRICTION_TIMESCALE: |
229 | m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); | 265 | m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); |
266 | m_angularMotor.FrictionTimescale = m_angularFrictionTimescale; | ||
230 | break; | 267 | break; |
231 | case Vehicle.ANGULAR_MOTOR_DIRECTION: | 268 | case Vehicle.ANGULAR_MOTOR_DIRECTION: |
232 | // Limit requested angular speed to 2 rps= 4 pi rads/sec | 269 | // Limit requested angular speed to 2 rps= 4 pi rads/sec |
233 | pValue.X = Math.Max(-12.56f, Math.Min(pValue.X, 12.56f)); | 270 | pValue.X = ClampInRange(-12.56f, pValue.X, 12.56f); |
234 | pValue.Y = Math.Max(-12.56f, Math.Min(pValue.Y, 12.56f)); | 271 | pValue.Y = ClampInRange(-12.56f, pValue.Y, 12.56f); |
235 | pValue.Z = Math.Max(-12.56f, Math.Min(pValue.Z, 12.56f)); | 272 | pValue.Z = ClampInRange(-12.56f, pValue.Z, 12.56f); |
236 | m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); | 273 | m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); |
237 | // m_angularMotorApply = 100; | 274 | m_angularMotor.Zero(); |
275 | m_angularMotor.SetTarget(m_angularMotorDirection); | ||
238 | break; | 276 | break; |
239 | case Vehicle.LINEAR_FRICTION_TIMESCALE: | 277 | case Vehicle.LINEAR_FRICTION_TIMESCALE: |
240 | m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); | 278 | m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); |
279 | m_linearMotor.FrictionTimescale = m_linearFrictionTimescale; | ||
241 | break; | 280 | break; |
242 | case Vehicle.LINEAR_MOTOR_DIRECTION: | 281 | case Vehicle.LINEAR_MOTOR_DIRECTION: |
243 | m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); | 282 | m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); |
244 | m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); | 283 | m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); |
284 | m_linearMotor.SetTarget(m_linearMotorDirection); | ||
245 | break; | 285 | break; |
246 | case Vehicle.LINEAR_MOTOR_OFFSET: | 286 | case Vehicle.LINEAR_MOTOR_OFFSET: |
247 | m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); | 287 | m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); |
@@ -303,7 +343,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
303 | m_VhoverEfficiency = 0; | 343 | m_VhoverEfficiency = 0; |
304 | m_VhoverTimescale = 0; | 344 | m_VhoverTimescale = 0; |
305 | m_VehicleBuoyancy = 0; | 345 | m_VehicleBuoyancy = 0; |
306 | 346 | ||
307 | m_linearDeflectionEfficiency = 1; | 347 | m_linearDeflectionEfficiency = 1; |
308 | m_linearDeflectionTimescale = 1; | 348 | m_linearDeflectionTimescale = 1; |
309 | 349 | ||
@@ -319,6 +359,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
319 | 359 | ||
320 | m_referenceFrame = Quaternion.Identity; | 360 | m_referenceFrame = Quaternion.Identity; |
321 | m_flags = (VehicleFlag)0; | 361 | m_flags = (VehicleFlag)0; |
362 | |||
322 | break; | 363 | break; |
323 | 364 | ||
324 | case Vehicle.TYPE_SLED: | 365 | case Vehicle.TYPE_SLED: |
@@ -351,10 +392,14 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
351 | m_bankingMix = 1; | 392 | m_bankingMix = 1; |
352 | 393 | ||
353 | m_referenceFrame = Quaternion.Identity; | 394 | m_referenceFrame = Quaternion.Identity; |
354 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP); | 395 | m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
355 | m_flags &= | 396 | | VehicleFlag.HOVER_TERRAIN_ONLY |
356 | ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | | 397 | | VehicleFlag.HOVER_GLOBAL_HEIGHT |
357 | VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); | 398 | | VehicleFlag.HOVER_UP_ONLY); |
399 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP | ||
400 | | VehicleFlag.LIMIT_ROLL_ONLY | ||
401 | | VehicleFlag.LIMIT_MOTOR_UP); | ||
402 | |||
358 | break; | 403 | break; |
359 | case Vehicle.TYPE_CAR: | 404 | case Vehicle.TYPE_CAR: |
360 | m_linearMotorDirection = Vector3.Zero; | 405 | m_linearMotorDirection = Vector3.Zero; |
@@ -498,6 +543,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
498 | m_bankingEfficiency = 0; | 543 | m_bankingEfficiency = 0; |
499 | m_bankingMix = 0.7f; | 544 | m_bankingMix = 0.7f; |
500 | m_bankingTimescale = 5; | 545 | m_bankingTimescale = 5; |
546 | |||
501 | m_referenceFrame = Quaternion.Identity; | 547 | m_referenceFrame = Quaternion.Identity; |
502 | 548 | ||
503 | m_referenceFrame = Quaternion.Identity; | 549 | m_referenceFrame = Quaternion.Identity; |
@@ -510,152 +556,467 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
510 | | VehicleFlag.HOVER_GLOBAL_HEIGHT); | 556 | | VehicleFlag.HOVER_GLOBAL_HEIGHT); |
511 | break; | 557 | break; |
512 | } | 558 | } |
559 | |||
560 | // Update any physical parameters based on this type. | ||
561 | Refresh(); | ||
562 | |||
563 | m_linearMotor = new BSVMotor("LinearMotor", m_linearMotorTimescale, | ||
564 | m_linearMotorDecayTimescale, m_linearFrictionTimescale, | ||
565 | 1f); | ||
566 | m_linearMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG DEBUG (enables detail logging) | ||
567 | |||
568 | m_angularMotor = new BSVMotor("AngularMotor", m_angularMotorTimescale, | ||
569 | m_angularMotorDecayTimescale, m_angularFrictionTimescale, | ||
570 | 1f); | ||
571 | m_angularMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG DEBUG (enables detail logging) | ||
572 | |||
573 | /* Not implemented | ||
574 | m_verticalAttractionMotor = new BSVMotor("VerticalAttraction", m_verticalAttractionTimescale, | ||
575 | BSMotor.Infinite, BSMotor.InfiniteVector, | ||
576 | m_verticalAttractionEfficiency); | ||
577 | // Z goes away and we keep X and Y | ||
578 | m_verticalAttractionMotor.FrictionTimescale = new Vector3(BSMotor.Infinite, BSMotor.Infinite, 0.1f); | ||
579 | m_verticalAttractionMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG DEBUG (enables detail logging) | ||
580 | */ | ||
581 | } | ||
582 | #endregion // Vehicle parameter setting | ||
583 | |||
584 | public void Refresh() | ||
585 | { | ||
586 | // If asking for a refresh, reset the physical parameters before the next simulation step. | ||
587 | PhysicsScene.PostTaintObject("BSDynamics.Refresh", Prim.LocalID, delegate() | ||
588 | { | ||
589 | SetPhysicalParameters(); | ||
590 | }); | ||
513 | } | 591 | } |
514 | 592 | ||
515 | // Some of the properties of this prim may have changed. | 593 | // Some of the properties of this prim may have changed. |
516 | // Do any updating needed for a vehicle | 594 | // Do any updating needed for a vehicle |
517 | public void Refresh() | 595 | private void SetPhysicalParameters() |
518 | { | 596 | { |
519 | if (IsActive) | 597 | if (IsActive) |
520 | { | 598 | { |
521 | // Friction effects are handled by this vehicle code | 599 | // Remember the mass so we don't have to fetch it every step |
522 | BulletSimAPI.SetFriction2(Prim.PhysBody.ptr, 0f); | 600 | m_vehicleMass = Prim.TotalMass; |
523 | BulletSimAPI.SetHitFraction2(Prim.PhysBody.ptr, 0f); | 601 | |
524 | 602 | // Friction affects are handled by this vehicle code | |
525 | // BulletSimAPI.SetAngularDamping2(Prim.PhysBody.ptr, 0.8f); | 603 | PhysicsScene.PE.SetFriction(Prim.PhysBody, BSParam.VehicleFriction); |
526 | 604 | PhysicsScene.PE.SetRestitution(Prim.PhysBody, BSParam.VehicleRestitution); | |
527 | VDetailLog("{0},BSDynamics.Refresh,zeroingFriction and adding damping", Prim.LocalID); | 605 | |
606 | // Moderate angular movement introduced by Bullet. | ||
607 | // TODO: possibly set AngularFactor and LinearFactor for the type of vehicle. | ||
608 | // Maybe compute linear and angular factor and damping from params. | ||
609 | PhysicsScene.PE.SetAngularDamping(Prim.PhysBody, BSParam.VehicleAngularDamping); | ||
610 | PhysicsScene.PE.SetLinearFactor(Prim.PhysBody, BSParam.VehicleLinearFactor); | ||
611 | PhysicsScene.PE.SetAngularFactorV(Prim.PhysBody, BSParam.VehicleAngularFactor); | ||
612 | |||
613 | // Vehicles report collision events so we know when it's on the ground | ||
614 | PhysicsScene.PE.AddToCollisionFlags(Prim.PhysBody, CollisionFlags.BS_VEHICLE_COLLISIONS); | ||
615 | |||
616 | Prim.Inertia = PhysicsScene.PE.CalculateLocalInertia(Prim.PhysShape, m_vehicleMass); | ||
617 | PhysicsScene.PE.SetMassProps(Prim.PhysBody, m_vehicleMass, Prim.Inertia); | ||
618 | PhysicsScene.PE.UpdateInertiaTensor(Prim.PhysBody); | ||
619 | |||
620 | // Set the gravity for the vehicle depending on the buoyancy | ||
621 | // TODO: what should be done if prim and vehicle buoyancy differ? | ||
622 | m_VehicleGravity = Prim.ComputeGravity(m_VehicleBuoyancy); | ||
623 | // The actual vehicle gravity is set to zero in Bullet so we can do all the application of same. | ||
624 | PhysicsScene.PE.SetGravity(Prim.PhysBody, Vector3.Zero); | ||
625 | |||
626 | VDetailLog("{0},BSDynamics.SetPhysicalParameters,mass={1},inert={2},vehGrav={3},aDamp={4},frict={5},rest={6},lFact={7},aFact={8}", | ||
627 | Prim.LocalID, m_vehicleMass, Prim.Inertia, m_VehicleGravity, | ||
628 | BSParam.VehicleAngularDamping, BSParam.VehicleFriction, BSParam.VehicleRestitution, | ||
629 | BSParam.VehicleLinearFactor, BSParam.VehicleAngularFactor | ||
630 | ); | ||
631 | } | ||
632 | else | ||
633 | { | ||
634 | if (Prim.PhysBody.HasPhysicalBody) | ||
635 | PhysicsScene.PE.RemoveFromCollisionFlags(Prim.PhysBody, CollisionFlags.BS_VEHICLE_COLLISIONS); | ||
528 | } | 636 | } |
529 | } | 637 | } |
530 | 638 | ||
531 | public bool RemoveBodyDependencies(BSPhysObject prim) | 639 | public bool RemoveBodyDependencies(BSPhysObject prim) |
532 | { | 640 | { |
533 | // If active, we need to add our properties back when the body is rebuilt. | 641 | Refresh(); |
534 | return IsActive; | 642 | return IsActive; |
535 | } | 643 | } |
536 | 644 | ||
537 | public void RestoreBodyDependencies(BSPhysObject prim) | 645 | #region Known vehicle value functions |
646 | // Vehicle physical parameters that we buffer from constant getting and setting. | ||
647 | // The "m_known*" values are unknown until they are fetched and the m_knownHas flag is set. | ||
648 | // Changing is remembered and the parameter is stored back into the physics engine only if updated. | ||
649 | // This does two things: 1) saves continuious calls into unmanaged code, and | ||
650 | // 2) signals when a physics property update must happen back to the simulator | ||
651 | // to update values modified for the vehicle. | ||
652 | private int m_knownChanged; | ||
653 | private int m_knownHas; | ||
654 | private float m_knownTerrainHeight; | ||
655 | private float m_knownWaterLevel; | ||
656 | private Vector3 m_knownPosition; | ||
657 | private Vector3 m_knownVelocity; | ||
658 | private Vector3 m_knownForce; | ||
659 | private Vector3 m_knownForceImpulse; | ||
660 | private Quaternion m_knownOrientation; | ||
661 | private Vector3 m_knownRotationalVelocity; | ||
662 | private Vector3 m_knownRotationalForce; | ||
663 | private Vector3 m_knownRotationalImpulse; | ||
664 | private Vector3 m_knownForwardVelocity; // vehicle relative forward speed | ||
665 | |||
666 | private const int m_knownChangedPosition = 1 << 0; | ||
667 | private const int m_knownChangedVelocity = 1 << 1; | ||
668 | private const int m_knownChangedForce = 1 << 2; | ||
669 | private const int m_knownChangedForceImpulse = 1 << 3; | ||
670 | private const int m_knownChangedOrientation = 1 << 4; | ||
671 | private const int m_knownChangedRotationalVelocity = 1 << 5; | ||
672 | private const int m_knownChangedRotationalForce = 1 << 6; | ||
673 | private const int m_knownChangedRotationalImpulse = 1 << 7; | ||
674 | private const int m_knownChangedTerrainHeight = 1 << 8; | ||
675 | private const int m_knownChangedWaterLevel = 1 << 9; | ||
676 | private const int m_knownChangedForwardVelocity = 1 <<10; | ||
677 | |||
678 | public void ForgetKnownVehicleProperties() | ||
679 | { | ||
680 | m_knownHas = 0; | ||
681 | m_knownChanged = 0; | ||
682 | } | ||
683 | // Push all the changed values back into the physics engine | ||
684 | public void PushKnownChanged() | ||
685 | { | ||
686 | if (m_knownChanged != 0) | ||
687 | { | ||
688 | if ((m_knownChanged & m_knownChangedPosition) != 0) | ||
689 | Prim.ForcePosition = m_knownPosition; | ||
690 | |||
691 | if ((m_knownChanged & m_knownChangedOrientation) != 0) | ||
692 | Prim.ForceOrientation = m_knownOrientation; | ||
693 | |||
694 | if ((m_knownChanged & m_knownChangedVelocity) != 0) | ||
695 | { | ||
696 | Prim.ForceVelocity = m_knownVelocity; | ||
697 | // Fake out Bullet by making it think the velocity is the same as last time. | ||
698 | // Bullet does a bunch of smoothing for changing parameters. | ||
699 | // Since the vehicle is demanding this setting, we override Bullet's smoothing | ||
700 | // by telling Bullet the value was the same last time. | ||
701 | // PhysicsScene.PE.SetInterpolationLinearVelocity(Prim.PhysBody, m_knownVelocity); | ||
702 | } | ||
703 | |||
704 | if ((m_knownChanged & m_knownChangedForce) != 0) | ||
705 | Prim.AddForce((Vector3)m_knownForce, false /*pushForce*/, true /*inTaintTime*/); | ||
706 | |||
707 | if ((m_knownChanged & m_knownChangedForceImpulse) != 0) | ||
708 | Prim.AddForceImpulse((Vector3)m_knownForceImpulse, false /*pushforce*/, true /*inTaintTime*/); | ||
709 | |||
710 | if ((m_knownChanged & m_knownChangedRotationalVelocity) != 0) | ||
711 | { | ||
712 | Prim.ForceRotationalVelocity = m_knownRotationalVelocity; | ||
713 | // PhysicsScene.PE.SetInterpolationAngularVelocity(Prim.PhysBody, m_knownRotationalVelocity); | ||
714 | } | ||
715 | |||
716 | if ((m_knownChanged & m_knownChangedRotationalImpulse) != 0) | ||
717 | Prim.ApplyTorqueImpulse((Vector3)m_knownRotationalImpulse, true /*inTaintTime*/); | ||
718 | |||
719 | if ((m_knownChanged & m_knownChangedRotationalForce) != 0) | ||
720 | { | ||
721 | Prim.AddAngularForce((Vector3)m_knownRotationalForce, false /*pushForce*/, true /*inTaintTime*/); | ||
722 | } | ||
723 | |||
724 | // If we set one of the values (ie, the physics engine didn't do it) we must force | ||
725 | // an UpdateProperties event to send the changes up to the simulator. | ||
726 | PhysicsScene.PE.PushUpdate(Prim.PhysBody); | ||
727 | } | ||
728 | m_knownChanged = 0; | ||
729 | } | ||
730 | |||
731 | // Since the computation of terrain height can be a little involved, this routine | ||
732 | // is used to fetch the height only once for each vehicle simulation step. | ||
733 | Vector3 lastRememberedHeightPos; | ||
734 | private float GetTerrainHeight(Vector3 pos) | ||
538 | { | 735 | { |
539 | if (Prim.LocalID != prim.LocalID) | 736 | if ((m_knownHas & m_knownChangedTerrainHeight) == 0 || pos != lastRememberedHeightPos) |
540 | { | 737 | { |
541 | // The call should be on us by our prim. Error if not. | 738 | lastRememberedHeightPos = pos; |
542 | PhysicsScene.Logger.ErrorFormat("{0} RestoreBodyDependencies: called by not my prim. passedLocalID={1}, vehiclePrimLocalID={2}", | 739 | m_knownTerrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos); |
543 | LogHeader, prim.LocalID, Prim.LocalID); | 740 | m_knownHas |= m_knownChangedTerrainHeight; |
544 | return; | 741 | } |
742 | return m_knownTerrainHeight; | ||
743 | } | ||
744 | |||
745 | // Since the computation of water level can be a little involved, this routine | ||
746 | // is used ot fetch the level only once for each vehicle simulation step. | ||
747 | private float GetWaterLevel(Vector3 pos) | ||
748 | { | ||
749 | if ((m_knownHas & m_knownChangedWaterLevel) == 0) | ||
750 | { | ||
751 | m_knownWaterLevel = Prim.PhysicsScene.TerrainManager.GetWaterLevelAtXYZ(pos); | ||
752 | m_knownHas |= m_knownChangedWaterLevel; | ||
753 | } | ||
754 | return (float)m_knownWaterLevel; | ||
755 | } | ||
756 | |||
757 | private Vector3 VehiclePosition | ||
758 | { | ||
759 | get | ||
760 | { | ||
761 | if ((m_knownHas & m_knownChangedPosition) == 0) | ||
762 | { | ||
763 | m_knownPosition = Prim.ForcePosition; | ||
764 | m_knownHas |= m_knownChangedPosition; | ||
765 | } | ||
766 | return m_knownPosition; | ||
767 | } | ||
768 | set | ||
769 | { | ||
770 | m_knownPosition = value; | ||
771 | m_knownChanged |= m_knownChangedPosition; | ||
772 | m_knownHas |= m_knownChangedPosition; | ||
545 | } | 773 | } |
546 | Refresh(); | ||
547 | } | 774 | } |
548 | 775 | ||
776 | private Quaternion VehicleOrientation | ||
777 | { | ||
778 | get | ||
779 | { | ||
780 | if ((m_knownHas & m_knownChangedOrientation) == 0) | ||
781 | { | ||
782 | m_knownOrientation = Prim.ForceOrientation; | ||
783 | m_knownHas |= m_knownChangedOrientation; | ||
784 | } | ||
785 | return m_knownOrientation; | ||
786 | } | ||
787 | set | ||
788 | { | ||
789 | m_knownOrientation = value; | ||
790 | m_knownChanged |= m_knownChangedOrientation; | ||
791 | m_knownHas |= m_knownChangedOrientation; | ||
792 | } | ||
793 | } | ||
794 | |||
795 | private Vector3 VehicleVelocity | ||
796 | { | ||
797 | get | ||
798 | { | ||
799 | if ((m_knownHas & m_knownChangedVelocity) == 0) | ||
800 | { | ||
801 | m_knownVelocity = Prim.ForceVelocity; | ||
802 | m_knownHas |= m_knownChangedVelocity; | ||
803 | } | ||
804 | return m_knownVelocity; | ||
805 | } | ||
806 | set | ||
807 | { | ||
808 | m_knownVelocity = value; | ||
809 | m_knownChanged |= m_knownChangedVelocity; | ||
810 | m_knownHas |= m_knownChangedVelocity; | ||
811 | } | ||
812 | } | ||
813 | |||
814 | private void VehicleAddForce(Vector3 pForce) | ||
815 | { | ||
816 | if ((m_knownHas & m_knownChangedForce) == 0) | ||
817 | { | ||
818 | m_knownForce = Vector3.Zero; | ||
819 | m_knownHas |= m_knownChangedForce; | ||
820 | } | ||
821 | m_knownForce += pForce; | ||
822 | m_knownChanged |= m_knownChangedForce; | ||
823 | } | ||
824 | |||
825 | private void VehicleAddForceImpulse(Vector3 pImpulse) | ||
826 | { | ||
827 | if ((m_knownHas & m_knownChangedForceImpulse) == 0) | ||
828 | { | ||
829 | m_knownForceImpulse = Vector3.Zero; | ||
830 | m_knownHas |= m_knownChangedForceImpulse; | ||
831 | } | ||
832 | m_knownForceImpulse += pImpulse; | ||
833 | m_knownChanged |= m_knownChangedForceImpulse; | ||
834 | } | ||
835 | |||
836 | private Vector3 VehicleRotationalVelocity | ||
837 | { | ||
838 | get | ||
839 | { | ||
840 | if ((m_knownHas & m_knownChangedRotationalVelocity) == 0) | ||
841 | { | ||
842 | m_knownRotationalVelocity = Prim.ForceRotationalVelocity; | ||
843 | m_knownHas |= m_knownChangedRotationalVelocity; | ||
844 | } | ||
845 | return (Vector3)m_knownRotationalVelocity; | ||
846 | } | ||
847 | set | ||
848 | { | ||
849 | m_knownRotationalVelocity = value; | ||
850 | m_knownChanged |= m_knownChangedRotationalVelocity; | ||
851 | m_knownHas |= m_knownChangedRotationalVelocity; | ||
852 | } | ||
853 | } | ||
854 | private void VehicleAddAngularForce(Vector3 aForce) | ||
855 | { | ||
856 | if ((m_knownHas & m_knownChangedRotationalForce) == 0) | ||
857 | { | ||
858 | m_knownRotationalForce = Vector3.Zero; | ||
859 | } | ||
860 | m_knownRotationalForce += aForce; | ||
861 | m_knownChanged |= m_knownChangedRotationalForce; | ||
862 | m_knownHas |= m_knownChangedRotationalForce; | ||
863 | } | ||
864 | private void VehicleAddRotationalImpulse(Vector3 pImpulse) | ||
865 | { | ||
866 | if ((m_knownHas & m_knownChangedRotationalImpulse) == 0) | ||
867 | { | ||
868 | m_knownRotationalImpulse = Vector3.Zero; | ||
869 | m_knownHas |= m_knownChangedRotationalImpulse; | ||
870 | } | ||
871 | m_knownRotationalImpulse += pImpulse; | ||
872 | m_knownChanged |= m_knownChangedRotationalImpulse; | ||
873 | } | ||
874 | |||
875 | // Vehicle relative forward velocity | ||
876 | private Vector3 VehicleForwardVelocity | ||
877 | { | ||
878 | get | ||
879 | { | ||
880 | if ((m_knownHas & m_knownChangedForwardVelocity) == 0) | ||
881 | { | ||
882 | m_knownForwardVelocity = VehicleVelocity * Quaternion.Inverse(Quaternion.Normalize(VehicleOrientation)); | ||
883 | m_knownHas |= m_knownChangedForwardVelocity; | ||
884 | } | ||
885 | return m_knownForwardVelocity; | ||
886 | } | ||
887 | } | ||
888 | private float VehicleForwardSpeed | ||
889 | { | ||
890 | get | ||
891 | { | ||
892 | return VehicleForwardVelocity.X; | ||
893 | } | ||
894 | } | ||
895 | |||
896 | #endregion // Known vehicle value functions | ||
897 | |||
549 | // One step of the vehicle properties for the next 'pTimestep' seconds. | 898 | // One step of the vehicle properties for the next 'pTimestep' seconds. |
550 | internal void Step(float pTimestep) | 899 | internal void Step(float pTimestep) |
551 | { | 900 | { |
552 | if (!IsActive) return; | 901 | if (!IsActive) return; |
553 | 902 | ||
554 | // DEBUG | 903 | ForgetKnownVehicleProperties(); |
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 | 904 | ||
565 | MoveLinear(pTimestep); | 905 | MoveLinear(pTimestep); |
566 | // Commented out for debug | ||
567 | MoveAngular(pTimestep); | 906 | MoveAngular(pTimestep); |
568 | // Prim.ApplyTorqueImpulse(-Prim.RotationalVelocity * m_vehicleMass, false); // DEBUG DEBUG | ||
569 | // Prim.ForceRotationalVelocity = -Prim.RotationalVelocity; // DEBUG DEBUG | ||
570 | 907 | ||
571 | LimitRotation(pTimestep); | 908 | LimitRotation(pTimestep); |
572 | 909 | ||
573 | // remember the position so next step we can limit absolute movement effects | 910 | // remember the position so next step we can limit absolute movement effects |
574 | m_lastPositionVector = Prim.ForcePosition; | 911 | m_lastPositionVector = VehiclePosition; |
575 | 912 | ||
576 | VDetailLog("{0},BSDynamics.Step,frict={1},grav={2},inertia={3},mass={4}", // DEBUG DEBUG | 913 | // If we forced the changing of some vehicle parameters, update the values and |
577 | Prim.LocalID, | 914 | // for the physics engine to note the changes so an UpdateProperties event will happen. |
578 | BulletSimAPI.GetFriction2(Prim.PhysBody.ptr), | 915 | PushKnownChanged(); |
579 | BulletSimAPI.GetGravity2(Prim.PhysBody.ptr), | 916 | |
580 | Prim.Inertia, | 917 | if (PhysicsScene.VehiclePhysicalLoggingEnabled) |
581 | m_vehicleMass | 918 | PhysicsScene.PE.DumpRigidBody(PhysicsScene.World, Prim.PhysBody); |
582 | ); | 919 | |
583 | VDetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}", | 920 | VDetailLog("{0},BSDynamics.Step,done,pos={1}, force={2},velocity={3},angvel={4}", |
584 | Prim.LocalID, Prim.ForcePosition, Prim.Force, Prim.ForceVelocity, Prim.RotationalVelocity); | 921 | Prim.LocalID, VehiclePosition, m_knownForce, VehicleVelocity, VehicleRotationalVelocity); |
585 | }// end Step | 922 | } |
586 | 923 | ||
587 | // Apply the effect of the linear motor. | 924 | // Called after the simulation step |
588 | // Also does hover and float. | 925 | internal void PostStep(float pTimestep) |
926 | { | ||
927 | if (!IsActive) return; | ||
928 | |||
929 | if (PhysicsScene.VehiclePhysicalLoggingEnabled) | ||
930 | PhysicsScene.PE.DumpRigidBody(PhysicsScene.World, Prim.PhysBody); | ||
931 | } | ||
932 | |||
933 | // Apply the effect of the linear motor and other linear motions (like hover and float). | ||
589 | private void MoveLinear(float pTimestep) | 934 | private void MoveLinear(float pTimestep) |
590 | { | 935 | { |
591 | // m_linearMotorDirection is the target direction we are moving relative to the vehicle coordinates | 936 | ComputeLinearVelocity(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 | 937 | ||
600 | // Add (desiredVelocity - lastAppliedVelocity) / howLongItShouldTakeToComplete | 938 | ComputeLinearTerrainHeightCorrection(pTimestep); |
601 | Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/(m_linearMotorTimescale) * pTimestep; | ||
602 | m_lastLinearVelocityVector += addAmount; | ||
603 | 939 | ||
604 | float decayFactor = (1.0f / m_linearMotorDecayTimescale) * pTimestep; | 940 | ComputeLinearHover(pTimestep); |
605 | m_linearMotorDirection *= (1f - decayFactor); | ||
606 | 941 | ||
607 | // Rotate new object velocity from vehicle relative to world coordinates | 942 | ComputeLinearBlockingEndPoint(pTimestep); |
608 | m_newVelocity = m_lastLinearVelocityVector * Prim.ForceOrientation; | ||
609 | 943 | ||
610 | // Apply friction for next time | 944 | ComputeLinearMotorUp(pTimestep); |
611 | Vector3 frictionFactor = (Vector3.One / m_linearFrictionTimescale) * pTimestep; | ||
612 | m_lastLinearVelocityVector *= (Vector3.One - frictionFactor); | ||
613 | 945 | ||
614 | VDetailLog("{0},MoveLinear,nonZero,origlmDir={1},origlvVel={2},vehVel={3},add={4},decay={5},frict={6},lmDir={7},lvVec={8},newVel={9}", | 946 | ApplyGravity(pTimestep); |
615 | Prim.LocalID, origDir, origVel, vehicleVelocity, addAmount, decayFactor, frictionFactor, | 947 | |
616 | m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity); | 948 | // If not changing some axis, reduce out velocity |
617 | } | 949 | if ((m_flags & (VehicleFlag.NO_X | VehicleFlag.NO_Y | VehicleFlag.NO_Z)) != 0) |
618 | else | ||
619 | { | 950 | { |
620 | // if what remains of direction is very small, zero it. | 951 | Vector3 vel = VehicleVelocity; |
621 | m_linearMotorDirection = Vector3.Zero; | 952 | if ((m_flags & (VehicleFlag.NO_X)) != 0) |
622 | m_lastLinearVelocityVector = Vector3.Zero; | 953 | vel.X = 0; |
623 | m_newVelocity = Vector3.Zero; | 954 | if ((m_flags & (VehicleFlag.NO_Y)) != 0) |
955 | vel.Y = 0; | ||
956 | if ((m_flags & (VehicleFlag.NO_Z)) != 0) | ||
957 | vel.Z = 0; | ||
958 | VehicleVelocity = vel; | ||
959 | } | ||
624 | 960 | ||
625 | VDetailLog("{0},MoveLinear,zeroed", Prim.LocalID); | 961 | // ================================================================== |
962 | // Clamp high or low velocities | ||
963 | float newVelocityLengthSq = VehicleVelocity.LengthSquared(); | ||
964 | if (newVelocityLengthSq > BSParam.VehicleMaxLinearVelocitySquared) | ||
965 | { | ||
966 | Vector3 origVelW = VehicleVelocity; // DEBUG DEBUG | ||
967 | VehicleVelocity /= VehicleVelocity.Length(); | ||
968 | VehicleVelocity *= BSParam.VehicleMaxLinearVelocity; | ||
969 | VDetailLog("{0}, MoveLinear,clampMax,origVelW={1},lenSq={2},maxVelSq={3},,newVelW={4}", | ||
970 | Prim.LocalID, origVelW, newVelocityLengthSq, BSParam.VehicleMaxLinearVelocitySquared, VehicleVelocity); | ||
626 | } | 971 | } |
972 | else if (newVelocityLengthSq < 0.001f) | ||
973 | VehicleVelocity = Vector3.Zero; | ||
627 | 974 | ||
628 | // m_newVelocity is velocity computed from linear motor in world coordinates | 975 | VDetailLog("{0}, MoveLinear,done,isColl={1},newVel={2}", Prim.LocalID, Prim.IsColliding, VehicleVelocity ); |
629 | 976 | ||
630 | // Gravity and Buoyancy | 977 | } // end MoveLinear() |
631 | // There is some gravity, make a gravity force vector that is applied after object velocity. | ||
632 | // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; | ||
633 | Vector3 grav = Prim.PhysicsScene.DefaultGravity * (1f - m_VehicleBuoyancy); | ||
634 | 978 | ||
635 | /* | 979 | public void ComputeLinearVelocity(float pTimestep) |
636 | * RA: Not sure why one would do this unless we are hoping external forces are doing gravity, ... | 980 | { |
637 | // Preserve the current Z velocity | 981 | // Step the motor from the current value. Get the correction needed this step. |
638 | Vector3 vel_now = m_prim.Velocity; | 982 | Vector3 origVelW = VehicleVelocity; // DEBUG |
639 | m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity | 983 | Vector3 currentVelV = VehicleVelocity * Quaternion.Inverse(VehicleOrientation); |
640 | */ | 984 | Vector3 linearMotorCorrectionV = m_linearMotor.Step(pTimestep, currentVelV); |
985 | |||
986 | // Motor is vehicle coordinates. Rotate it to world coordinates | ||
987 | Vector3 linearMotorVelocityW = linearMotorCorrectionV * VehicleOrientation; | ||
988 | |||
989 | // If we're a ground vehicle, don't add any upward Z movement | ||
990 | if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0) | ||
991 | { | ||
992 | if (linearMotorVelocityW.Z > 0f) | ||
993 | linearMotorVelocityW.Z = 0f; | ||
994 | } | ||
995 | |||
996 | // Add this correction to the velocity to make it faster/slower. | ||
997 | VehicleVelocity += linearMotorVelocityW; | ||
641 | 998 | ||
642 | Vector3 pos = Prim.ForcePosition; | 999 | VDetailLog("{0}, MoveLinear,velocity,origVelW={1},velV={2},correctV={3},correctW={4},newVelW={5}", |
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); | 1000 | Prim.LocalID, origVelW, currentVelV, linearMotorCorrectionV, linearMotorVelocityW, VehicleVelocity); |
1001 | } | ||
644 | 1002 | ||
1003 | public void ComputeLinearTerrainHeightCorrection(float pTimestep) | ||
1004 | { | ||
645 | // If below the terrain, move us above the ground a little. | 1005 | // If below the terrain, move us above the ground a little. |
646 | float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos); | 1006 | // TODO: Consider taking the rotated size of the object or possibly casting a ray. |
647 | // Taking the rotated size doesn't work here because m_prim.Size is the size of the root prim and not the linkset. | 1007 | if (VehiclePosition.Z < GetTerrainHeight(VehiclePosition)) |
648 | // TODO: Add a m_prim.LinkSet.Size similar to m_prim.LinkSet.Mass. | ||
649 | // Vector3 rotatedSize = m_prim.Size * m_prim.ForceOrientation; | ||
650 | // if (rotatedSize.Z < terrainHeight) | ||
651 | if (pos.Z < terrainHeight) | ||
652 | { | 1008 | { |
653 | pos.Z = terrainHeight + 2; | 1009 | // Force position because applying force won't get the vehicle through the terrain |
654 | Prim.ForcePosition = pos; | 1010 | Vector3 newPosition = VehiclePosition; |
655 | VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, terrainHeight, pos); | 1011 | newPosition.Z = GetTerrainHeight(VehiclePosition) + 1f; |
1012 | VehiclePosition = newPosition; | ||
1013 | VDetailLog("{0}, MoveLinear,terrainHeight,terrainHeight={1},pos={2}", | ||
1014 | Prim.LocalID, GetTerrainHeight(VehiclePosition), VehiclePosition); | ||
656 | } | 1015 | } |
1016 | } | ||
657 | 1017 | ||
658 | // Check if hovering | 1018 | public void ComputeLinearHover(float pTimestep) |
1019 | { | ||
659 | // m_VhoverEfficiency: 0=bouncy, 1=totally damped | 1020 | // m_VhoverEfficiency: 0=bouncy, 1=totally damped |
660 | // m_VhoverTimescale: time to achieve height | 1021 | // m_VhoverTimescale: time to achieve height |
661 | if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) | 1022 | if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) |
@@ -663,11 +1024,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
663 | // We should hover, get the target height | 1024 | // We should hover, get the target height |
664 | if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0) | 1025 | if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0) |
665 | { | 1026 | { |
666 | m_VhoverTargetHeight = Prim.PhysicsScene.GetWaterLevelAtXYZ(pos) + m_VhoverHeight; | 1027 | m_VhoverTargetHeight = GetWaterLevel(VehiclePosition) + m_VhoverHeight; |
667 | } | 1028 | } |
668 | if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) | 1029 | if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) |
669 | { | 1030 | { |
670 | m_VhoverTargetHeight = terrainHeight + m_VhoverHeight; | 1031 | m_VhoverTargetHeight = GetTerrainHeight(VehiclePosition) + m_VhoverHeight; |
671 | } | 1032 | } |
672 | if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0) | 1033 | if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0) |
673 | { | 1034 | { |
@@ -677,45 +1038,63 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
677 | if ((m_flags & VehicleFlag.HOVER_UP_ONLY) != 0) | 1038 | if ((m_flags & VehicleFlag.HOVER_UP_ONLY) != 0) |
678 | { | 1039 | { |
679 | // If body is already heigher, use its height as target height | 1040 | // If body is already heigher, use its height as target height |
680 | if (pos.Z > m_VhoverTargetHeight) | 1041 | if (VehiclePosition.Z > m_VhoverTargetHeight) |
681 | m_VhoverTargetHeight = pos.Z; | 1042 | m_VhoverTargetHeight = VehiclePosition.Z; |
682 | } | 1043 | } |
1044 | |||
683 | if ((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0) | 1045 | if ((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0) |
684 | { | 1046 | { |
685 | if (Math.Abs(pos.Z - m_VhoverTargetHeight) > 0.2f) | 1047 | if (Math.Abs(VehiclePosition.Z - m_VhoverTargetHeight) > 0.2f) |
686 | { | 1048 | { |
1049 | Vector3 pos = VehiclePosition; | ||
687 | pos.Z = m_VhoverTargetHeight; | 1050 | pos.Z = m_VhoverTargetHeight; |
688 | Prim.ForcePosition = pos; | 1051 | VehiclePosition = pos; |
1052 | |||
1053 | VDetailLog("{0}, MoveLinear,hover,pos={1},lockHoverHeight", Prim.LocalID, pos); | ||
689 | } | 1054 | } |
690 | } | 1055 | } |
691 | else | 1056 | else |
692 | { | 1057 | { |
693 | float verticalError = pos.Z - m_VhoverTargetHeight; | 1058 | // Error is positive if below the target and negative if above. |
694 | // RA: where does the 50 come from? | 1059 | Vector3 hpos = VehiclePosition; |
695 | float verticalCorrectionVelocity = pTimestep * ((verticalError * 50.0f) / m_VhoverTimescale); | 1060 | float verticalError = m_VhoverTargetHeight - hpos.Z; |
696 | // Replace Vertical speed with correction figure if significant | 1061 | float verticalCorrection = verticalError / m_VhoverTimescale; |
697 | if (Math.Abs(verticalError) > 0.01f) | 1062 | verticalCorrection *= m_VhoverEfficiency; |
698 | { | 1063 | |
699 | m_newVelocity.Z += verticalCorrectionVelocity; | 1064 | hpos.Z += verticalCorrection; |
700 | //KF: m_VhoverEfficiency is not yet implemented | 1065 | VehiclePosition = hpos; |
701 | } | 1066 | |
702 | else if (verticalError < -0.01) | 1067 | // Since we are hovering, we need to do the opposite of falling -- get rid of world Z |
703 | { | 1068 | Vector3 vel = VehicleVelocity; |
704 | m_newVelocity.Z -= verticalCorrectionVelocity; | 1069 | vel.Z = 0f; |
705 | } | 1070 | VehicleVelocity = vel; |
706 | else | 1071 | |
707 | { | 1072 | /* |
708 | m_newVelocity.Z = 0f; | 1073 | float verticalCorrectionVelocity = verticalError / m_VhoverTimescale; |
709 | } | 1074 | Vector3 verticalCorrection = new Vector3(0f, 0f, verticalCorrectionVelocity); |
1075 | verticalCorrection *= m_vehicleMass; | ||
1076 | |||
1077 | // TODO: implement m_VhoverEfficiency correctly | ||
1078 | VehicleAddForceImpulse(verticalCorrection); | ||
1079 | */ | ||
1080 | |||
1081 | VDetailLog("{0}, MoveLinear,hover,pos={1},eff={2},hoverTS={3},height={4},target={5},err={6},corr={7}", | ||
1082 | Prim.LocalID, VehiclePosition, m_VhoverEfficiency, | ||
1083 | m_VhoverTimescale, m_VhoverHeight, m_VhoverTargetHeight, | ||
1084 | verticalError, verticalCorrection); | ||
710 | } | 1085 | } |
711 | 1086 | ||
712 | VDetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}", Prim.LocalID, pos, m_newVelocity, m_VhoverHeight, m_VhoverTargetHeight); | ||
713 | } | 1087 | } |
1088 | } | ||
714 | 1089 | ||
1090 | public bool ComputeLinearBlockingEndPoint(float pTimestep) | ||
1091 | { | ||
1092 | bool changed = false; | ||
1093 | |||
1094 | Vector3 pos = VehiclePosition; | ||
715 | Vector3 posChange = pos - m_lastPositionVector; | 1095 | Vector3 posChange = pos - m_lastPositionVector; |
716 | if (m_BlockingEndPoint != Vector3.Zero) | 1096 | if (m_BlockingEndPoint != Vector3.Zero) |
717 | { | 1097 | { |
718 | bool changed = false; | ||
719 | if (pos.X >= (m_BlockingEndPoint.X - (float)1)) | 1098 | if (pos.X >= (m_BlockingEndPoint.X - (float)1)) |
720 | { | 1099 | { |
721 | pos.X -= posChange.X + 1; | 1100 | pos.X -= posChange.X + 1; |
@@ -743,233 +1122,118 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
743 | } | 1122 | } |
744 | if (changed) | 1123 | if (changed) |
745 | { | 1124 | { |
746 | Prim.ForcePosition = pos; | 1125 | VehiclePosition = pos; |
747 | VDetailLog("{0},MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}", | 1126 | VDetailLog("{0}, MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}", |
748 | Prim.LocalID, m_BlockingEndPoint, posChange, pos); | 1127 | Prim.LocalID, m_BlockingEndPoint, posChange, pos); |
749 | } | 1128 | } |
750 | } | 1129 | } |
1130 | return changed; | ||
1131 | } | ||
751 | 1132 | ||
752 | #region downForce | 1133 | // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags : |
753 | Vector3 downForce = Vector3.Zero; | 1134 | // Prevent ground vehicles from motoring into the sky. This flag has a subtle effect when |
754 | 1135 | // used with conjunction with banking: the strength of the banking will decay when the | |
1136 | // vehicle no longer experiences collisions. The decay timescale is the same as | ||
1137 | // VEHICLE_BANKING_TIMESCALE. This is to help prevent ground vehicles from steering | ||
1138 | // when they are in mid jump. | ||
1139 | // TODO: this code is wrong. Also, what should it do for boats (height from water)? | ||
1140 | // This is just using the ground and a general collision check. Should really be using | ||
1141 | // a downward raycast to find what is below. | ||
1142 | public void ComputeLinearMotorUp(float pTimestep) | ||
1143 | { | ||
755 | if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0) | 1144 | if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0) |
756 | { | 1145 | { |
757 | // If the vehicle is motoring into the sky, get it going back down. | 1146 | // This code tries to decide if the object is not on the ground and then pushing down |
758 | // Is this an angular force or both linear and angular?? | 1147 | /* |
759 | float distanceAboveGround = pos.Z - terrainHeight; | 1148 | float targetHeight = Type == Vehicle.TYPE_BOAT ? GetWaterLevel(VehiclePosition) : GetTerrainHeight(VehiclePosition); |
760 | if (distanceAboveGround > 2f) | 1149 | distanceAboveGround = VehiclePosition.Z - targetHeight; |
1150 | // Not colliding if the vehicle is off the ground | ||
1151 | if (!Prim.IsColliding) | ||
761 | { | 1152 | { |
762 | // downForce = new Vector3(0, 0, (-distanceAboveGround / m_bankingTimescale) * pTimestep); | ||
763 | // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale); | 1153 | // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale); |
764 | downForce = new Vector3(0, 0, -distanceAboveGround); | 1154 | VehicleVelocity += new Vector3(0, 0, -distanceAboveGround); |
765 | } | 1155 | } |
766 | // TODO: this calculation is all wrong. From the description at | 1156 | // TODO: this calculation is wrong. From the description at |
767 | // (http://wiki.secondlife.com/wiki/Category:LSL_Vehicle), the downForce | 1157 | // (http://wiki.secondlife.com/wiki/Category:LSL_Vehicle), the downForce |
768 | // has a decay factor. This says this force should | 1158 | // has a decay factor. This says this force should |
769 | // be computed with a motor. | 1159 | // be computed with a motor. |
770 | VDetailLog("{0},MoveLinear,limitMotorUp,distAbove={1},downForce={2}", | 1160 | // TODO: add interaction with banking. |
771 | Prim.LocalID, distanceAboveGround, downForce); | 1161 | VDetailLog("{0}, MoveLinear,limitMotorUp,distAbove={1},colliding={2},ret={3}", |
772 | } | 1162 | Prim.LocalID, distanceAboveGround, Prim.IsColliding, ret); |
773 | #endregion // downForce | 1163 | */ |
774 | 1164 | ||
775 | // If not changing some axis, reduce out velocity | 1165 | // Another approach is to measure if we're going up. If going up and not colliding, |
776 | if ((m_flags & (VehicleFlag.NO_X)) != 0) | 1166 | // the vehicle is in the air. Fix that by pushing down. |
777 | m_newVelocity.X = 0; | 1167 | if (!Prim.IsColliding && VehicleVelocity.Z > 0.1) |
778 | if ((m_flags & (VehicleFlag.NO_Y)) != 0) | 1168 | { |
779 | m_newVelocity.Y = 0; | 1169 | // Get rid of any of the velocity vector that is pushing us up. |
780 | if ((m_flags & (VehicleFlag.NO_Z)) != 0) | 1170 | float upVelocity = VehicleVelocity.Z; |
781 | m_newVelocity.Z = 0; | 1171 | VehicleVelocity += new Vector3(0, 0, -upVelocity); |
782 | 1172 | ||
783 | // Clamp REALLY high or low velocities | 1173 | /* |
784 | if (m_newVelocity.LengthSquared() > 1e6f) | 1174 | // If we're pointed up into the air, we should nose down |
785 | { | 1175 | Vector3 pointingDirection = Vector3.UnitX * VehicleOrientation; |
786 | m_newVelocity /= m_newVelocity.Length(); | 1176 | // The rotation around the Y axis is pitch up or down |
787 | m_newVelocity *= 1000f; | 1177 | if (pointingDirection.Y > 0.01f) |
1178 | { | ||
1179 | float angularCorrectionForce = -(float)Math.Asin(pointingDirection.Y); | ||
1180 | Vector3 angularCorrectionVector = new Vector3(0f, angularCorrectionForce, 0f); | ||
1181 | // Rotate into world coordinates and apply to vehicle | ||
1182 | angularCorrectionVector *= VehicleOrientation; | ||
1183 | VehicleAddAngularForce(angularCorrectionVector); | ||
1184 | VDetailLog("{0}, MoveLinear,limitMotorUp,newVel={1},pntDir={2},corrFrc={3},aCorr={4}", | ||
1185 | Prim.LocalID, VehicleVelocity, pointingDirection, angularCorrectionForce, angularCorrectionVector); | ||
1186 | } | ||
1187 | */ | ||
1188 | VDetailLog("{0}, MoveLinear,limitMotorUp,collide={1},upVel={2},newVel={3}", | ||
1189 | Prim.LocalID, Prim.IsColliding, upVelocity, VehicleVelocity); | ||
1190 | } | ||
788 | } | 1191 | } |
789 | else if (m_newVelocity.LengthSquared() < 1e-6f) | 1192 | } |
790 | m_newVelocity = Vector3.Zero; | ||
791 | 1193 | ||
792 | // Stuff new linear velocity into the vehicle | 1194 | private void ApplyGravity(float pTimeStep) |
793 | Prim.ForceVelocity = m_newVelocity; | 1195 | { |
794 | // Prim.ApplyForceImpulse((m_newVelocity - Prim.Velocity) * m_vehicleMass, false); // DEBUG DEBUG | 1196 | Vector3 appliedGravity = m_VehicleGravity * m_vehicleMass; |
795 | 1197 | ||
796 | Vector3 totalDownForce = downForce + grav; | 1198 | // Hack to reduce downward force if the vehicle is probably sitting on the ground |
797 | if (totalDownForce != Vector3.Zero) | 1199 | if (Prim.IsColliding && IsGroundVehicle) |
798 | { | 1200 | appliedGravity *= BSParam.VehicleGroundGravityFudge; |
799 | Prim.AddForce(totalDownForce * m_vehicleMass, false); | ||
800 | // Prim.ApplyForceImpulse(totalDownForce * m_vehicleMass, false); | ||
801 | } | ||
802 | 1201 | ||
803 | VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},primVel={4},totalDown={5}", | 1202 | VehicleAddForce(appliedGravity); |
804 | Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity, Prim.Velocity, totalDownForce); | ||
805 | 1203 | ||
806 | } // end MoveLinear() | 1204 | VDetailLog("{0}, MoveLinear,applyGravity,vehGrav={1},collid={2},appliedForce={3}", |
1205 | Prim.LocalID, m_VehicleGravity, Prim.IsColliding, appliedGravity); | ||
1206 | } | ||
807 | 1207 | ||
808 | // ======================================================================= | 1208 | // ======================================================================= |
1209 | // ======================================================================= | ||
809 | // Apply the effect of the angular motor. | 1210 | // Apply the effect of the angular motor. |
1211 | // The 'contribution' is how much angular correction velocity each function wants. | ||
1212 | // All the contributions are added together and the resulting velocity is | ||
1213 | // set directly on the vehicle. | ||
810 | private void MoveAngular(float pTimestep) | 1214 | private void MoveAngular(float pTimestep) |
811 | { | 1215 | { |
812 | // m_angularMotorDirection // angular velocity requested by LSL motor | 1216 | ComputeAngularTurning(pTimestep); |
813 | // m_angularMotorApply // application frame counter | ||
814 | // m_angularMotorVelocity // current angular motor velocity (ramps up and down) | ||
815 | // m_angularMotorTimescale // motor angular velocity ramp up rate | ||
816 | // m_angularMotorDecayTimescale // motor angular velocity decay rate | ||
817 | // m_angularFrictionTimescale // body angular velocity decay rate | ||
818 | // m_lastAngularVelocity // what was last applied to body | ||
819 | |||
820 | if (m_angularMotorDirection.LengthSquared() > 0.0001) | ||
821 | { | ||
822 | Vector3 origVel = m_angularMotorVelocity; | ||
823 | Vector3 origDir = m_angularMotorDirection; | ||
824 | |||
825 | // new velocity += error / ( time to get there / step interval) | ||
826 | // requested direction - current vehicle direction | ||
827 | m_angularMotorVelocity += (m_angularMotorDirection - m_angularMotorVelocity) / (m_angularMotorTimescale / pTimestep); | ||
828 | // decay requested direction | ||
829 | m_angularMotorDirection *= (1.0f - (pTimestep * 1.0f/m_angularMotorDecayTimescale)); | ||
830 | |||
831 | VDetailLog("{0},MoveAngular,angularMotorApply,angTScale={1},timeStep={2},origvel={3},origDir={4},vel={5}", | ||
832 | Prim.LocalID, m_angularMotorTimescale, pTimestep, origVel, origDir, m_angularMotorVelocity); | ||
833 | } | ||
834 | else | ||
835 | { | ||
836 | m_angularMotorVelocity = Vector3.Zero; | ||
837 | } | ||
838 | |||
839 | #region Vertical attactor | ||
840 | 1217 | ||
841 | Vector3 vertattr = Vector3.Zero; | 1218 | ComputeAngularVerticalAttraction(); |
842 | Vector3 deflection = Vector3.Zero; | ||
843 | Vector3 banking = Vector3.Zero; | ||
844 | 1219 | ||
845 | // If vertical attaction timescale is reasonable and we applied an angular force last time... | 1220 | ComputeAngularDeflection(); |
846 | if (m_verticalAttractionTimescale < 300 && m_lastAngularVelocity != Vector3.Zero) | ||
847 | { | ||
848 | float VAservo = pTimestep * 0.2f / m_verticalAttractionTimescale; | ||
849 | if (Prim.IsColliding) | ||
850 | VAservo = pTimestep * 0.05f / (m_verticalAttractionTimescale); | ||
851 | |||
852 | VAservo *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency); | ||
853 | |||
854 | // Create a vector of the vehicle "up" in world coordinates | ||
855 | Vector3 verticalError = Vector3.UnitZ * Prim.ForceOrientation; | ||
856 | // verticalError.X and .Y are the World error amounts. They are 0 when there is no | ||
857 | // error (Vehicle Body is 'vertical'), and .Z will be 1. As the body leans to its | ||
858 | // side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall | ||
859 | // and .Z will go // negative. Similar for tilt and |.Y|. .X and .Y must be | ||
860 | // modulated to prevent a stable inverted body. | ||
861 | |||
862 | // Error is 0 (no error) to +/- 2 (max error) | ||
863 | if (verticalError.Z < 0.0f) | ||
864 | { | ||
865 | verticalError.X = 2.0f - verticalError.X; | ||
866 | verticalError.Y = 2.0f - verticalError.Y; | ||
867 | } | ||
868 | // scale it by VAservo (timestep and timescale) | ||
869 | verticalError = verticalError * VAservo; | ||
870 | |||
871 | // 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. | ||
873 | // Z is not changed. | ||
874 | vertattr.X = verticalError.Y; | ||
875 | vertattr.Y = - verticalError.X; | ||
876 | vertattr.Z = 0f; | ||
877 | |||
878 | // scaling appears better usingsquare-law | ||
879 | Vector3 angularVelocity = Prim.ForceRotationalVelocity; | ||
880 | float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency); | ||
881 | vertattr.X += bounce * angularVelocity.X; | ||
882 | vertattr.Y += bounce * angularVelocity.Y; | ||
883 | 1221 | ||
884 | VDetailLog("{0},MoveAngular,verticalAttraction,VAservo={1},effic={2},verticalError={3},bounce={4},vertattr={5}", | 1222 | ComputeAngularBanking(); |
885 | Prim.LocalID, VAservo, m_verticalAttractionEfficiency, verticalError, bounce, vertattr); | ||
886 | 1223 | ||
887 | } | 1224 | // ================================================================== |
888 | #endregion // Vertical attactor | 1225 | if (VehicleRotationalVelocity.ApproxEquals(Vector3.Zero, 0.0001f)) |
889 | |||
890 | #region Deflection | ||
891 | |||
892 | if (m_angularDeflectionEfficiency != 0) | ||
893 | { | 1226 | { |
894 | // Compute a scaled vector that points in the preferred axis (X direction) | 1227 | // The vehicle is not adding anything angular wise. |
895 | Vector3 scaledDefaultDirection = | 1228 | VehicleRotationalVelocity = Vector3.Zero; |
896 | new Vector3((pTimestep * 10 * (m_angularDeflectionEfficiency / m_angularDeflectionTimescale)), 0, 0); | 1229 | VDetailLog("{0}, MoveAngular,done,zero", Prim.LocalID); |
897 | // Adding the current vehicle orientation and reference frame displaces the orientation to the frame. | ||
898 | // Rotate the scaled default axix relative to the actual vehicle direction giving where it should point. | ||
899 | Vector3 preferredAxisOfMotion = scaledDefaultDirection * Quaternion.Add(Prim.ForceOrientation, m_referenceFrame); | ||
900 | |||
901 | // Scale by efficiency and timescale | ||
902 | deflection = (preferredAxisOfMotion * (m_angularDeflectionEfficiency) / m_angularDeflectionTimescale) * pTimestep; | ||
903 | |||
904 | VDetailLog("{0},MoveAngular,Deflection,perfAxis={1},deflection={2}", | ||
905 | Prim.LocalID, preferredAxisOfMotion, deflection); | ||
906 | // This deflection computation is not correct. | ||
907 | deflection = Vector3.Zero; | ||
908 | } | 1230 | } |
909 | 1231 | else | |
910 | #endregion | ||
911 | |||
912 | #region Banking | ||
913 | |||
914 | if (m_bankingEfficiency != 0) | ||
915 | { | 1232 | { |
916 | Vector3 dir = Vector3.One * Prim.ForceOrientation; | 1233 | VDetailLog("{0}, MoveAngular,done,nonZero,angVel={1}", Prim.LocalID, VehicleRotationalVelocity); |
917 | float mult = (m_bankingMix*m_bankingMix)*-1*(m_bankingMix < 0 ? -1 : 1); | ||
918 | //Changes which way it banks in and out of turns | ||
919 | |||
920 | //Use the square of the efficiency, as it looks much more how SL banking works | ||
921 | float effSquared = (m_bankingEfficiency*m_bankingEfficiency); | ||
922 | if (m_bankingEfficiency < 0) | ||
923 | effSquared *= -1; //Keep the negative! | ||
924 | |||
925 | float mix = Math.Abs(m_bankingMix); | ||
926 | if (m_angularMotorVelocity.X == 0) | ||
927 | { | ||
928 | /*if (!parent.Orientation.ApproxEquals(this.m_referenceFrame, 0.25f)) | ||
929 | { | ||
930 | Vector3 axisAngle; | ||
931 | float angle; | ||
932 | parent.Orientation.GetAxisAngle(out axisAngle, out angle); | ||
933 | Vector3 rotatedVel = parent.Velocity * parent.Orientation; | ||
934 | if ((rotatedVel.X < 0 && axisAngle.Y > 0) || (rotatedVel.X > 0 && axisAngle.Y < 0)) | ||
935 | m_angularMotorVelocity.X += (effSquared * (mult * mix)) * (1f) * 10; | ||
936 | else | ||
937 | m_angularMotorVelocity.X += (effSquared * (mult * mix)) * (-1f) * 10; | ||
938 | }*/ | ||
939 | } | ||
940 | else | ||
941 | banking.Z += (effSquared*(mult*mix))*(m_angularMotorVelocity.X) * 4; | ||
942 | if (!Prim.IsColliding && Math.Abs(m_angularMotorVelocity.X) > mix) | ||
943 | //If they are colliding, we probably shouldn't shove the prim around... probably | ||
944 | { | ||
945 | float angVelZ = m_angularMotorVelocity.X*-1; | ||
946 | /*if(angVelZ > mix) | ||
947 | angVelZ = mix; | ||
948 | else if(angVelZ < -mix) | ||
949 | angVelZ = -mix;*/ | ||
950 | //This controls how fast and how far the banking occurs | ||
951 | Vector3 bankingRot = new Vector3(angVelZ*(effSquared*mult), 0, 0); | ||
952 | if (bankingRot.X > 3) | ||
953 | bankingRot.X = 3; | ||
954 | else if (bankingRot.X < -3) | ||
955 | bankingRot.X = -3; | ||
956 | bankingRot *= Prim.ForceOrientation; | ||
957 | banking += bankingRot; | ||
958 | } | ||
959 | m_angularMotorVelocity.X *= m_bankingEfficiency == 1 ? 0.0f : 1 - m_bankingEfficiency; | ||
960 | VDetailLog("{0},MoveAngular,Banking,bEff={1},angMotVel={2},banking={3}", | ||
961 | Prim.LocalID, m_bankingEfficiency, m_angularMotorVelocity, banking); | ||
962 | } | 1234 | } |
963 | 1235 | ||
964 | #endregion | 1236 | // ================================================================== |
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 | 1237 | //Offset section |
974 | if (m_linearMotorOffset != Vector3.Zero) | 1238 | if (m_linearMotorOffset != Vector3.Zero) |
975 | { | 1239 | { |
@@ -985,8 +1249,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
985 | // | 1249 | // |
986 | // The torque created is the linear velocity crossed with the offset | 1250 | // The torque created is the linear velocity crossed with the offset |
987 | 1251 | ||
988 | // NOTE: this computation does should be in the linear section | 1252 | // TODO: this computation should be in the linear section |
989 | // because there we know the impulse being applied. | 1253 | // because that is where we know the impulse being applied. |
990 | Vector3 torqueFromOffset = Vector3.Zero; | 1254 | Vector3 torqueFromOffset = Vector3.Zero; |
991 | // torqueFromOffset = Vector3.Cross(m_linearMotorOffset, appliedImpulse); | 1255 | // torqueFromOffset = Vector3.Cross(m_linearMotorOffset, appliedImpulse); |
992 | if (float.IsNaN(torqueFromOffset.X)) | 1256 | if (float.IsNaN(torqueFromOffset.X)) |
@@ -995,47 +1259,215 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
995 | torqueFromOffset.Y = 0; | 1259 | torqueFromOffset.Y = 0; |
996 | if (float.IsNaN(torqueFromOffset.Z)) | 1260 | if (float.IsNaN(torqueFromOffset.Z)) |
997 | torqueFromOffset.Z = 0; | 1261 | torqueFromOffset.Z = 0; |
998 | torqueFromOffset *= m_vehicleMass; | 1262 | |
999 | Prim.ApplyTorqueImpulse(torqueFromOffset, true); | 1263 | VehicleAddAngularForce(torqueFromOffset * m_vehicleMass); |
1000 | VDetailLog("{0},BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset); | 1264 | VDetailLog("{0}, BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset); |
1001 | } | 1265 | } |
1002 | 1266 | ||
1003 | #endregion | 1267 | } |
1004 | 1268 | ||
1269 | private void ComputeAngularTurning(float pTimestep) | ||
1270 | { | ||
1271 | // The user wants this many radians per second angular change? | ||
1272 | Vector3 currentAngularV = VehicleRotationalVelocity * Quaternion.Inverse(VehicleOrientation); | ||
1273 | Vector3 angularMotorContributionV = m_angularMotor.Step(pTimestep, currentAngularV); | ||
1274 | |||
1275 | // ================================================================== | ||
1276 | // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags : | ||
1277 | // This flag prevents linear deflection parallel to world z-axis. This is useful | ||
1278 | // for preventing ground vehicles with large linear deflection, like bumper cars, | ||
1279 | // from climbing their linear deflection into the sky. | ||
1280 | // That is, NO_DEFLECTION_UP says angular motion should not add any pitch or roll movement | ||
1281 | // TODO: This is here because this is where ODE put it but documentation says it | ||
1282 | // is a linear effect. Where should this check go? | ||
1005 | if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0) | 1283 | if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0) |
1006 | { | 1284 | { |
1007 | m_lastAngularVelocity.X = 0; | 1285 | angularMotorContributionV.X = 0f; |
1008 | m_lastAngularVelocity.Y = 0; | 1286 | angularMotorContributionV.Y = 0f; |
1009 | VDetailLog("{0},MoveAngular,noDeflectionUp,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity); | ||
1010 | } | 1287 | } |
1011 | 1288 | ||
1012 | if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f)) | 1289 | VehicleRotationalVelocity += angularMotorContributionV * VehicleOrientation; |
1290 | VDetailLog("{0}, MoveAngular,angularTurning,angularMotorContrib={1}", Prim.LocalID, angularMotorContributionV); | ||
1291 | } | ||
1292 | |||
1293 | // From http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial: | ||
1294 | // Some vehicles, like boats, should always keep their up-side up. This can be done by | ||
1295 | // enabling the "vertical attractor" behavior that springs the vehicle's local z-axis to | ||
1296 | // the world z-axis (a.k.a. "up"). To take advantage of this feature you would set the | ||
1297 | // VEHICLE_VERTICAL_ATTRACTION_TIMESCALE to control the period of the spring frequency, | ||
1298 | // and then set the VEHICLE_VERTICAL_ATTRACTION_EFFICIENCY to control the damping. An | ||
1299 | // efficiency of 0.0 will cause the spring to wobble around its equilibrium, while an | ||
1300 | // efficiency of 1.0 will cause the spring to reach its equilibrium with exponential decay. | ||
1301 | public void ComputeAngularVerticalAttraction() | ||
1302 | { | ||
1303 | // If vertical attaction timescale is reasonable | ||
1304 | if (enableAngularVerticalAttraction && m_verticalAttractionTimescale < m_verticalAttractionCutoff) | ||
1013 | { | 1305 | { |
1014 | m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero. | 1306 | Vector3 vertContributionV = Vector3.Zero; |
1015 | Prim.ZeroAngularMotion(true); | 1307 | Vector3 origRotVelW = VehicleRotationalVelocity; // DEBUG DEBUG |
1016 | VDetailLog("{0},MoveAngular,zeroAngularMotion,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity); | 1308 | |
1309 | // Take a vector pointing up and convert it from world to vehicle relative coords. | ||
1310 | Vector3 verticalError = Vector3.UnitZ * VehicleOrientation; | ||
1311 | |||
1312 | // If vertical attraction correction is needed, the vector that was pointing up (UnitZ) | ||
1313 | // is now: | ||
1314 | // leaning to one side: rotated around the X axis with the Y value going | ||
1315 | // from zero (nearly straight up) to one (completely to the side)) or | ||
1316 | // leaning front-to-back: rotated around the Y axis with the value of X being between | ||
1317 | // zero and one. | ||
1318 | // The value of Z is how far the rotation is off with 1 meaning none and 0 being 90 degrees. | ||
1319 | |||
1320 | // Y error means needed rotation around X axis and visa versa. | ||
1321 | // Since the error goes from zero to one, the asin is the corresponding angle. | ||
1322 | vertContributionV.X = (float)Math.Asin(verticalError.Y); | ||
1323 | // (Tilt forward (positive X) needs to tilt back (rotate negative) around Y axis.) | ||
1324 | vertContributionV.Y = -(float)Math.Asin(verticalError.X); | ||
1325 | |||
1326 | // If verticalError.Z is negative, the vehicle is upside down. Add additional push. | ||
1327 | if (verticalError.Z < 0f) | ||
1328 | { | ||
1329 | vertContributionV.X += Math.Sign(vertContributionV.X) * PIOverFour; | ||
1330 | // vertContribution.Y -= PIOverFour; | ||
1331 | } | ||
1332 | |||
1333 | // 'vertContrbution' is now the necessary angular correction to correct tilt in one second. | ||
1334 | // Correction happens over a number of seconds. | ||
1335 | Vector3 unscaledContribVerticalErrorV = vertContributionV; // DEBUG DEBUG | ||
1336 | vertContributionV /= m_verticalAttractionTimescale; | ||
1337 | |||
1338 | VehicleRotationalVelocity += vertContributionV * VehicleOrientation; | ||
1339 | |||
1340 | VDetailLog("{0}, MoveAngular,verticalAttraction,,origRotVW={1},vertError={2},unscaledV={3},eff={4},ts={5},vertContribV={6}", | ||
1341 | Prim.LocalID, origRotVelW, verticalError, unscaledContribVerticalErrorV, | ||
1342 | m_verticalAttractionEfficiency, m_verticalAttractionTimescale, vertContributionV); | ||
1017 | } | 1343 | } |
1018 | else | 1344 | } |
1345 | |||
1346 | // Angular correction to correct the direction the vehicle is pointing to be | ||
1347 | // the direction is should want to be pointing. | ||
1348 | // The vehicle is moving in some direction and correct its orientation to it is pointing | ||
1349 | // in that direction. | ||
1350 | // TODO: implement reference frame. | ||
1351 | public void ComputeAngularDeflection() | ||
1352 | { | ||
1353 | // Since angularMotorUp and angularDeflection are computed independently, they will calculate | ||
1354 | // approximately the same X or Y correction. When added together (when contributions are combined) | ||
1355 | // this creates an over-correction and then wabbling as the target is overshot. | ||
1356 | // TODO: rethink how the different correction computations inter-relate. | ||
1357 | |||
1358 | if (enableAngularDeflection && m_angularDeflectionEfficiency != 0 && VehicleForwardSpeed > 0.2) | ||
1359 | { | ||
1360 | Vector3 deflectContributionV = Vector3.Zero; | ||
1361 | |||
1362 | // The direction the vehicle is moving | ||
1363 | Vector3 movingDirection = VehicleVelocity; | ||
1364 | movingDirection.Normalize(); | ||
1365 | |||
1366 | // If the vehicle is going backward, it is still pointing forward | ||
1367 | movingDirection *= Math.Sign(VehicleForwardSpeed); | ||
1368 | |||
1369 | // The direction the vehicle is pointing | ||
1370 | Vector3 pointingDirection = Vector3.UnitX * VehicleOrientation; | ||
1371 | pointingDirection.Normalize(); | ||
1372 | |||
1373 | // The difference between what is and what should be. | ||
1374 | Vector3 deflectionError = movingDirection - pointingDirection; | ||
1375 | |||
1376 | // Don't try to correct very large errors (not our job) | ||
1377 | // if (Math.Abs(deflectionError.X) > PIOverFour) deflectionError.X = PIOverTwo * Math.Sign(deflectionError.X); | ||
1378 | // if (Math.Abs(deflectionError.Y) > PIOverFour) deflectionError.Y = PIOverTwo * Math.Sign(deflectionError.Y); | ||
1379 | // if (Math.Abs(deflectionError.Z) > PIOverFour) deflectionError.Z = PIOverTwo * Math.Sign(deflectionError.Z); | ||
1380 | if (Math.Abs(deflectionError.X) > PIOverFour) deflectionError.X = 0f; | ||
1381 | if (Math.Abs(deflectionError.Y) > PIOverFour) deflectionError.Y = 0f; | ||
1382 | if (Math.Abs(deflectionError.Z) > PIOverFour) deflectionError.Z = 0f; | ||
1383 | |||
1384 | // ret = m_angularDeflectionCorrectionMotor(1f, deflectionError); | ||
1385 | |||
1386 | // Scale the correction by recovery timescale and efficiency | ||
1387 | deflectContributionV = (-deflectionError) * m_angularDeflectionEfficiency; | ||
1388 | deflectContributionV /= m_angularDeflectionTimescale; | ||
1389 | |||
1390 | VehicleRotationalVelocity += deflectContributionV * VehicleOrientation; | ||
1391 | |||
1392 | VDetailLog("{0}, MoveAngular,Deflection,movingDir={1},pointingDir={2},deflectError={3},ret={4}", | ||
1393 | Prim.LocalID, movingDirection, pointingDirection, deflectionError, deflectContributionV); | ||
1394 | VDetailLog("{0}, MoveAngular,Deflection,fwdSpd={1},defEff={2},defTS={3}", | ||
1395 | Prim.LocalID, VehicleForwardSpeed, m_angularDeflectionEfficiency, m_angularDeflectionTimescale); | ||
1396 | } | ||
1397 | } | ||
1398 | |||
1399 | // Angular change to rotate the vehicle around the Z axis when the vehicle | ||
1400 | // is tipped around the X axis. | ||
1401 | // From http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial: | ||
1402 | // The vertical attractor feature must be enabled in order for the banking behavior to | ||
1403 | // function. The way banking works is this: a rotation around the vehicle's roll-axis will | ||
1404 | // produce a angular velocity around the yaw-axis, causing the vehicle to turn. The magnitude | ||
1405 | // of the yaw effect will be proportional to the | ||
1406 | // VEHICLE_BANKING_EFFICIENCY, the angle of the roll rotation, and sometimes the vehicle's | ||
1407 | // velocity along its preferred axis of motion. | ||
1408 | // The VEHICLE_BANKING_EFFICIENCY can vary between -1 and +1. When it is positive then any | ||
1409 | // positive rotation (by the right-hand rule) about the roll-axis will effect a | ||
1410 | // (negative) torque around the yaw-axis, making it turn to the right--that is the | ||
1411 | // vehicle will lean into the turn, which is how real airplanes and motorcycle's work. | ||
1412 | // Negating the banking coefficient will make it so that the vehicle leans to the | ||
1413 | // outside of the turn (not very "physical" but might allow interesting vehicles so why not?). | ||
1414 | // The VEHICLE_BANKING_MIX is a fake (i.e. non-physical) parameter that is useful for making | ||
1415 | // banking vehicles do what you want rather than what the laws of physics allow. | ||
1416 | // For example, consider a real motorcycle...it must be moving forward in order for | ||
1417 | // it to turn while banking, however video-game motorcycles are often configured | ||
1418 | // to turn in place when at a dead stop--because they are often easier to control | ||
1419 | // that way using the limited interface of the keyboard or game controller. The | ||
1420 | // VEHICLE_BANKING_MIX enables combinations of both realistic and non-realistic | ||
1421 | // banking by functioning as a slider between a banking that is correspondingly | ||
1422 | // totally static (0.0) and totally dynamic (1.0). By "static" we mean that the | ||
1423 | // banking effect depends only on the vehicle's rotation about its roll-axis compared | ||
1424 | // to "dynamic" where the banking is also proportional to its velocity along its | ||
1425 | // roll-axis. Finding the best value of the "mixture" will probably require trial and error. | ||
1426 | // The time it takes for the banking behavior to defeat a preexisting angular velocity about the | ||
1427 | // world z-axis is determined by the VEHICLE_BANKING_TIMESCALE. So if you want the vehicle to | ||
1428 | // bank quickly then give it a banking timescale of about a second or less, otherwise you can | ||
1429 | // make a sluggish vehicle by giving it a timescale of several seconds. | ||
1430 | public void ComputeAngularBanking() | ||
1431 | { | ||
1432 | if (enableAngularBanking && m_bankingEfficiency != 0 && m_verticalAttractionTimescale < m_verticalAttractionCutoff) | ||
1019 | { | 1433 | { |
1020 | // Apply to the body. | 1434 | Vector3 bankingContributionV = Vector3.Zero; |
1021 | // The above calculates the absolute angular velocity needed. Angular velocity is massless. | 1435 | |
1022 | // Since we are stuffing the angular velocity directly into the object, the computed | 1436 | // Rotate a UnitZ vector (pointing up) to how the vehicle is oriented. |
1023 | // velocity needs to be scaled by the timestep. | 1437 | // As the vehicle rolls to the right or left, the Y value will increase from |
1024 | Vector3 applyAngularForce = ((m_lastAngularVelocity * pTimestep) - Prim.ForceRotationalVelocity); | 1438 | // zero (straight up) to 1 or -1 (full tilt right or left) |
1025 | Prim.ForceRotationalVelocity = applyAngularForce; | 1439 | Vector3 rollComponents = Vector3.UnitZ * VehicleOrientation; |
1026 | 1440 | ||
1027 | // Decay the angular movement for next time | 1441 | // Figure out the yaw value for this much roll. |
1028 | Vector3 decayamount = (Vector3.One / m_angularFrictionTimescale) * pTimestep; | 1442 | // Squared because that seems to give a good value |
1029 | m_lastAngularVelocity *= Vector3.One - decayamount; | 1443 | float yawAngle = (float)Math.Asin(rollComponents.Y * rollComponents.Y) * m_bankingEfficiency; |
1030 | 1444 | ||
1031 | VDetailLog("{0},MoveAngular,done,newRotVel={1},decay={2},lastAngular={3}", | 1445 | // actual error = static turn error + dynamic turn error |
1032 | Prim.LocalID, applyAngularForce, decayamount, m_lastAngularVelocity); | 1446 | float mixedYawAngle = yawAngle * (1f - m_bankingMix) + yawAngle * m_bankingMix * VehicleForwardSpeed; |
1447 | |||
1448 | // TODO: the banking effect should not go to infinity but what to limit it to? | ||
1449 | mixedYawAngle = ClampInRange(-20f, mixedYawAngle, 20f); | ||
1450 | |||
1451 | // Build the force vector to change rotation from what it is to what it should be | ||
1452 | bankingContributionV.Z = -mixedYawAngle; | ||
1453 | |||
1454 | // Don't do it all at once. | ||
1455 | bankingContributionV /= m_bankingTimescale; | ||
1456 | |||
1457 | VehicleRotationalVelocity += bankingContributionV * VehicleOrientation; | ||
1458 | |||
1459 | VDetailLog("{0}, MoveAngular,Banking,rollComp={1},speed={2},rollComp={3},yAng={4},mYAng={5},ret={6}", | ||
1460 | Prim.LocalID, rollComponents, VehicleForwardSpeed, rollComponents, yawAngle, mixedYawAngle, bankingContributionV); | ||
1033 | } | 1461 | } |
1034 | } //end MoveAngular | 1462 | } |
1035 | 1463 | ||
1464 | // This is from previous instantiations of XXXDynamics.cs. | ||
1465 | // Applies roll reference frame. | ||
1466 | // TODO: is this the right way to separate the code to do this operation? | ||
1467 | // Should this be in MoveAngular()? | ||
1036 | internal void LimitRotation(float timestep) | 1468 | internal void LimitRotation(float timestep) |
1037 | { | 1469 | { |
1038 | Quaternion rotq = Prim.ForceOrientation; | 1470 | Quaternion rotq = VehicleOrientation; |
1039 | Quaternion m_rot = rotq; | 1471 | Quaternion m_rot = rotq; |
1040 | if (m_RollreferenceFrame != Quaternion.Identity) | 1472 | if (m_RollreferenceFrame != Quaternion.Identity) |
1041 | { | 1473 | { |
@@ -1063,12 +1495,18 @@ namespace OpenSim.Region.Physics.BulletSPlugin | |||
1063 | } | 1495 | } |
1064 | if (rotq != m_rot) | 1496 | if (rotq != m_rot) |
1065 | { | 1497 | { |
1066 | Prim.ForceOrientation = m_rot; | 1498 | VehicleOrientation = m_rot; |
1067 | VDetailLog("{0},LimitRotation,done,orig={1},new={2}", Prim.LocalID, rotq, m_rot); | 1499 | VDetailLog("{0}, LimitRotation,done,orig={1},new={2}", Prim.LocalID, rotq, m_rot); |
1068 | } | 1500 | } |
1069 | 1501 | ||
1070 | } | 1502 | } |
1071 | 1503 | ||
1504 | private float ClampInRange(float low, float val, float high) | ||
1505 | { | ||
1506 | return Math.Max(low, Math.Min(val, high)); | ||
1507 | // return Utils.Clamp(val, low, high); | ||
1508 | } | ||
1509 | |||
1072 | // Invoke the detailed logger and output something if it's enabled. | 1510 | // Invoke the detailed logger and output something if it's enabled. |
1073 | private void VDetailLog(string msg, params Object[] args) | 1511 | private void VDetailLog(string msg, params Object[] args) |
1074 | { | 1512 | { |