diff options
Diffstat (limited to 'OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs')
-rw-r--r-- | OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs | 1096 |
1 files changed, 1096 insertions, 0 deletions
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs new file mode 100644 index 0000000..c3b4dd8 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs | |||
@@ -0,0 +1,1096 @@ | |||
1 | /* | ||
2 | * Copyright (c) Contributors, http://opensimulator.org/ | ||
3 | * See CONTRIBUTORS.TXT for a full list of copyright holders. | ||
4 | * | ||
5 | * Redistribution and use in source and binary forms, with or without | ||
6 | * modification, are permitted provided that the following conditions are met: | ||
7 | * * Redistributions of source code must retain the above copyright | ||
8 | * notice, this list of conditions and the following disclaimer. | ||
9 | * * Redistributions in binary form must reproduce the above copyright | ||
10 | * notice, this list of conditions and the following disclaimer in the | ||
11 | * documentation and/or other materials provided with the distribution. | ||
12 | * * Neither the name of the OpenSimulator Project nor the | ||
13 | * names of its contributors may be used to endorse or promote products | ||
14 | * derived from this software without specific prior written permission. | ||
15 | * | ||
16 | * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY | ||
17 | * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED | ||
18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE | ||
19 | * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY | ||
20 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES | ||
21 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
22 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND | ||
23 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
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. | ||
26 | */ | ||
27 | |||
28 | /* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces | ||
29 | * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: | ||
30 | * ODEPrim.cs contains methods dealing with Prim editing, Prim | ||
31 | * characteristics and Kinetic motion. | ||
32 | * ODEDynamics.cs contains methods dealing with Prim Physical motion | ||
33 | * (dynamics) and the associated settings. Old Linear and angular | ||
34 | * motors for dynamic motion have been replace with MoveLinear() | ||
35 | * and MoveAngular(); 'Physical' is used only to switch ODE dynamic | ||
36 | * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to | ||
37 | * switch between 'VEHICLE' parameter use and general dynamics | ||
38 | * settings use. | ||
39 | */ | ||
40 | |||
41 | // Extensive change Ubit 2012 | ||
42 | |||
43 | using System; | ||
44 | using System.Collections.Generic; | ||
45 | using System.Reflection; | ||
46 | using System.Runtime.InteropServices; | ||
47 | using log4net; | ||
48 | using OpenMetaverse; | ||
49 | using OdeAPI; | ||
50 | using OpenSim.Framework; | ||
51 | using OpenSim.Region.PhysicsModules.SharedBase; | ||
52 | |||
53 | namespace OpenSim.Region.PhysicsModule.ubOde | ||
54 | { | ||
55 | public class ODEDynamics | ||
56 | { | ||
57 | public Vehicle Type | ||
58 | { | ||
59 | get { return m_type; } | ||
60 | } | ||
61 | |||
62 | private OdePrim rootPrim; | ||
63 | private ODEScene _pParentScene; | ||
64 | |||
65 | // Vehicle properties | ||
66 | // WARNING this are working copies for internel use | ||
67 | // their values may not be the corresponding parameter | ||
68 | |||
69 | private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier | ||
70 | private Quaternion m_RollreferenceFrame = Quaternion.Identity; // what hell is this ? | ||
71 | |||
72 | private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind | ||
73 | |||
74 | private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings: | ||
75 | // HOVER_TERRAIN_ONLY | ||
76 | // HOVER_GLOBAL_HEIGHT | ||
77 | // NO_DEFLECTION_UP | ||
78 | // HOVER_WATER_ONLY | ||
79 | // HOVER_UP_ONLY | ||
80 | // LIMIT_MOTOR_UP | ||
81 | // LIMIT_ROLL_ONLY | ||
82 | private Vector3 m_BlockingEndPoint = Vector3.Zero; // not sl | ||
83 | |||
84 | // Linear properties | ||
85 | private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time | ||
86 | private Vector3 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); | ||
87 | private float m_linearMotorDecayTimescale = 120; | ||
88 | private float m_linearMotorTimescale = 1000; | ||
89 | private Vector3 m_linearMotorOffset = Vector3.Zero; | ||
90 | |||
91 | //Angular properties | ||
92 | private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor | ||
93 | private float m_angularMotorTimescale = 1000; // motor angular velocity ramp up rate | ||
94 | private float m_angularMotorDecayTimescale = 120; // motor angular velocity decay rate | ||
95 | private Vector3 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); // body angular velocity decay rate | ||
96 | |||
97 | //Deflection properties | ||
98 | private float m_angularDeflectionEfficiency = 0; | ||
99 | private float m_angularDeflectionTimescale = 1000; | ||
100 | private float m_linearDeflectionEfficiency = 0; | ||
101 | private float m_linearDeflectionTimescale = 1000; | ||
102 | |||
103 | //Banking properties | ||
104 | private float m_bankingEfficiency = 0; | ||
105 | private float m_bankingMix = 0; | ||
106 | private float m_bankingTimescale = 1000; | ||
107 | |||
108 | //Hover and Buoyancy properties | ||
109 | private float m_VhoverHeight = 0f; | ||
110 | private float m_VhoverEfficiency = 0f; | ||
111 | private float m_VhoverTimescale = 1000f; | ||
112 | private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle. | ||
113 | // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) | ||
114 | // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. | ||
115 | // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. | ||
116 | |||
117 | //Attractor properties | ||
118 | private float m_verticalAttractionEfficiency = 1.0f; // damped | ||
119 | private float m_verticalAttractionTimescale = 1000f; // Timescale > 300 means no vert attractor. | ||
120 | |||
121 | |||
122 | // auxiliar | ||
123 | private float m_lmEfect = 0f; // current linear motor eficiency | ||
124 | private float m_lmDecay = 0f; // current linear decay | ||
125 | |||
126 | private float m_amEfect = 0; // current angular motor eficiency | ||
127 | private float m_amDecay = 0f; // current linear decay | ||
128 | |||
129 | private float m_ffactor = 1.0f; | ||
130 | |||
131 | private float m_timestep = 0.02f; | ||
132 | private float m_invtimestep = 50; | ||
133 | |||
134 | |||
135 | float m_ampwr; | ||
136 | float m_amdampX; | ||
137 | float m_amdampY; | ||
138 | float m_amdampZ; | ||
139 | |||
140 | float m_gravmod; | ||
141 | |||
142 | public float FrictionFactor | ||
143 | { | ||
144 | get | ||
145 | { | ||
146 | return m_ffactor; | ||
147 | } | ||
148 | } | ||
149 | |||
150 | public float GravMod | ||
151 | { | ||
152 | set | ||
153 | { | ||
154 | m_gravmod = value; | ||
155 | } | ||
156 | } | ||
157 | |||
158 | |||
159 | public ODEDynamics(OdePrim rootp) | ||
160 | { | ||
161 | rootPrim = rootp; | ||
162 | _pParentScene = rootPrim._parent_scene; | ||
163 | m_timestep = _pParentScene.ODE_STEPSIZE; | ||
164 | m_invtimestep = 1.0f / m_timestep; | ||
165 | m_gravmod = rootPrim.GravModifier; | ||
166 | } | ||
167 | |||
168 | public void DoSetVehicle(VehicleData vd) | ||
169 | { | ||
170 | m_type = vd.m_type; | ||
171 | m_flags = vd.m_flags; | ||
172 | |||
173 | |||
174 | // Linear properties | ||
175 | m_linearMotorDirection = vd.m_linearMotorDirection; | ||
176 | |||
177 | m_linearFrictionTimescale = vd.m_linearFrictionTimescale; | ||
178 | if (m_linearFrictionTimescale.X < m_timestep) m_linearFrictionTimescale.X = m_timestep; | ||
179 | if (m_linearFrictionTimescale.Y < m_timestep) m_linearFrictionTimescale.Y = m_timestep; | ||
180 | if (m_linearFrictionTimescale.Z < m_timestep) m_linearFrictionTimescale.Z = m_timestep; | ||
181 | |||
182 | m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale; | ||
183 | if (m_linearMotorDecayTimescale < m_timestep) m_linearMotorDecayTimescale = m_timestep; | ||
184 | m_linearMotorDecayTimescale += 0.2f; | ||
185 | m_linearMotorDecayTimescale *= m_invtimestep; | ||
186 | |||
187 | m_linearMotorTimescale = vd.m_linearMotorTimescale; | ||
188 | if (m_linearMotorTimescale < m_timestep) m_linearMotorTimescale = m_timestep; | ||
189 | |||
190 | m_linearMotorOffset = vd.m_linearMotorOffset; | ||
191 | |||
192 | //Angular properties | ||
193 | m_angularMotorDirection = vd.m_angularMotorDirection; | ||
194 | m_angularMotorTimescale = vd.m_angularMotorTimescale; | ||
195 | if (m_angularMotorTimescale < m_timestep) m_angularMotorTimescale = m_timestep; | ||
196 | |||
197 | m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale; | ||
198 | if (m_angularMotorDecayTimescale < m_timestep) m_angularMotorDecayTimescale = m_timestep; | ||
199 | m_angularMotorDecayTimescale *= m_invtimestep; | ||
200 | |||
201 | m_angularFrictionTimescale = vd.m_angularFrictionTimescale; | ||
202 | if (m_angularFrictionTimescale.X < m_timestep) m_angularFrictionTimescale.X = m_timestep; | ||
203 | if (m_angularFrictionTimescale.Y < m_timestep) m_angularFrictionTimescale.Y = m_timestep; | ||
204 | if (m_angularFrictionTimescale.Z < m_timestep) m_angularFrictionTimescale.Z = m_timestep; | ||
205 | |||
206 | //Deflection properties | ||
207 | m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency; | ||
208 | m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale; | ||
209 | if (m_angularDeflectionTimescale < m_timestep) m_angularDeflectionTimescale = m_timestep; | ||
210 | |||
211 | m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency; | ||
212 | m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale; | ||
213 | if (m_linearDeflectionTimescale < m_timestep) m_linearDeflectionTimescale = m_timestep; | ||
214 | |||
215 | //Banking properties | ||
216 | m_bankingEfficiency = vd.m_bankingEfficiency; | ||
217 | m_bankingMix = vd.m_bankingMix; | ||
218 | m_bankingTimescale = vd.m_bankingTimescale; | ||
219 | if (m_bankingTimescale < m_timestep) m_bankingTimescale = m_timestep; | ||
220 | |||
221 | //Hover and Buoyancy properties | ||
222 | m_VhoverHeight = vd.m_VhoverHeight; | ||
223 | m_VhoverEfficiency = vd.m_VhoverEfficiency; | ||
224 | m_VhoverTimescale = vd.m_VhoverTimescale; | ||
225 | if (m_VhoverTimescale < m_timestep) m_VhoverTimescale = m_timestep; | ||
226 | |||
227 | m_VehicleBuoyancy = vd.m_VehicleBuoyancy; | ||
228 | |||
229 | //Attractor properties | ||
230 | m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency; | ||
231 | m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale; | ||
232 | if (m_verticalAttractionTimescale < m_timestep) m_verticalAttractionTimescale = m_timestep; | ||
233 | |||
234 | // Axis | ||
235 | m_referenceFrame = vd.m_referenceFrame; | ||
236 | |||
237 | m_lmEfect = 0; | ||
238 | m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale); | ||
239 | m_amEfect = 0; | ||
240 | m_ffactor = 1.0f; | ||
241 | } | ||
242 | |||
243 | internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) | ||
244 | { | ||
245 | float len; | ||
246 | |||
247 | switch (pParam) | ||
248 | { | ||
249 | case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY: | ||
250 | if (pValue < 0f) pValue = 0f; | ||
251 | if (pValue > 1f) pValue = 1f; | ||
252 | m_angularDeflectionEfficiency = pValue; | ||
253 | break; | ||
254 | case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: | ||
255 | if (pValue < m_timestep) pValue = m_timestep; | ||
256 | m_angularDeflectionTimescale = pValue; | ||
257 | break; | ||
258 | case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: | ||
259 | if (pValue < m_timestep) pValue = m_timestep; | ||
260 | else if (pValue > 120) pValue = 120; | ||
261 | m_angularMotorDecayTimescale = pValue * m_invtimestep; | ||
262 | m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; | ||
263 | break; | ||
264 | case Vehicle.ANGULAR_MOTOR_TIMESCALE: | ||
265 | if (pValue < m_timestep) pValue = m_timestep; | ||
266 | m_angularMotorTimescale = pValue; | ||
267 | break; | ||
268 | case Vehicle.BANKING_EFFICIENCY: | ||
269 | if (pValue < -1f) pValue = -1f; | ||
270 | if (pValue > 1f) pValue = 1f; | ||
271 | m_bankingEfficiency = pValue; | ||
272 | break; | ||
273 | case Vehicle.BANKING_MIX: | ||
274 | if (pValue < 0f) pValue = 0f; | ||
275 | if (pValue > 1f) pValue = 1f; | ||
276 | m_bankingMix = pValue; | ||
277 | break; | ||
278 | case Vehicle.BANKING_TIMESCALE: | ||
279 | if (pValue < m_timestep) pValue = m_timestep; | ||
280 | m_bankingTimescale = pValue; | ||
281 | break; | ||
282 | case Vehicle.BUOYANCY: | ||
283 | if (pValue < -1f) pValue = -1f; | ||
284 | if (pValue > 1f) pValue = 1f; | ||
285 | m_VehicleBuoyancy = pValue; | ||
286 | break; | ||
287 | case Vehicle.HOVER_EFFICIENCY: | ||
288 | if (pValue < 0f) pValue = 0f; | ||
289 | if (pValue > 1f) pValue = 1f; | ||
290 | m_VhoverEfficiency = pValue; | ||
291 | break; | ||
292 | case Vehicle.HOVER_HEIGHT: | ||
293 | m_VhoverHeight = pValue; | ||
294 | break; | ||
295 | case Vehicle.HOVER_TIMESCALE: | ||
296 | if (pValue < m_timestep) pValue = m_timestep; | ||
297 | m_VhoverTimescale = pValue; | ||
298 | break; | ||
299 | case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: | ||
300 | if (pValue < 0f) pValue = 0f; | ||
301 | if (pValue > 1f) pValue = 1f; | ||
302 | m_linearDeflectionEfficiency = pValue; | ||
303 | break; | ||
304 | case Vehicle.LINEAR_DEFLECTION_TIMESCALE: | ||
305 | if (pValue < m_timestep) pValue = m_timestep; | ||
306 | m_linearDeflectionTimescale = pValue; | ||
307 | break; | ||
308 | case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: | ||
309 | if (pValue < m_timestep) pValue = m_timestep; | ||
310 | else if (pValue > 120) pValue = 120; | ||
311 | m_linearMotorDecayTimescale = (0.2f +pValue) * m_invtimestep; | ||
312 | m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale); | ||
313 | break; | ||
314 | case Vehicle.LINEAR_MOTOR_TIMESCALE: | ||
315 | if (pValue < m_timestep) pValue = m_timestep; | ||
316 | m_linearMotorTimescale = pValue; | ||
317 | break; | ||
318 | case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: | ||
319 | if (pValue < 0f) pValue = 0f; | ||
320 | if (pValue > 1f) pValue = 1f; | ||
321 | m_verticalAttractionEfficiency = pValue; | ||
322 | break; | ||
323 | case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: | ||
324 | if (pValue < m_timestep) pValue = m_timestep; | ||
325 | m_verticalAttractionTimescale = pValue; | ||
326 | break; | ||
327 | |||
328 | // These are vector properties but the engine lets you use a single float value to | ||
329 | // set all of the components to the same value | ||
330 | case Vehicle.ANGULAR_FRICTION_TIMESCALE: | ||
331 | if (pValue < m_timestep) pValue = m_timestep; | ||
332 | m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); | ||
333 | break; | ||
334 | case Vehicle.ANGULAR_MOTOR_DIRECTION: | ||
335 | m_angularMotorDirection = new Vector3(pValue, pValue, pValue); | ||
336 | len = m_angularMotorDirection.Length(); | ||
337 | if (len > 12.566f) | ||
338 | m_angularMotorDirection *= (12.566f / len); | ||
339 | |||
340 | m_amEfect = 1.0f ; // turn it on | ||
341 | m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; | ||
342 | |||
343 | if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) | ||
344 | && !rootPrim.m_isSelected && !rootPrim.m_disabled) | ||
345 | d.BodyEnable(rootPrim.Body); | ||
346 | break; | ||
347 | case Vehicle.LINEAR_FRICTION_TIMESCALE: | ||
348 | if (pValue < m_timestep) pValue = m_timestep; | ||
349 | m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); | ||
350 | break; | ||
351 | case Vehicle.LINEAR_MOTOR_DIRECTION: | ||
352 | m_linearMotorDirection = new Vector3(pValue, pValue, pValue); | ||
353 | len = m_linearMotorDirection.Length(); | ||
354 | if (len > 100.0f) | ||
355 | m_linearMotorDirection *= (100.0f / len); | ||
356 | |||
357 | m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale; | ||
358 | m_lmEfect = 1.0f; // turn it on | ||
359 | |||
360 | m_ffactor = 0.0f; | ||
361 | if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) | ||
362 | && !rootPrim.m_isSelected && !rootPrim.m_disabled) | ||
363 | d.BodyEnable(rootPrim.Body); | ||
364 | break; | ||
365 | case Vehicle.LINEAR_MOTOR_OFFSET: | ||
366 | m_linearMotorOffset = new Vector3(pValue, pValue, pValue); | ||
367 | len = m_linearMotorOffset.Length(); | ||
368 | if (len > 100.0f) | ||
369 | m_linearMotorOffset *= (100.0f / len); | ||
370 | break; | ||
371 | } | ||
372 | }//end ProcessFloatVehicleParam | ||
373 | |||
374 | internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) | ||
375 | { | ||
376 | float len; | ||
377 | |||
378 | switch (pParam) | ||
379 | { | ||
380 | case Vehicle.ANGULAR_FRICTION_TIMESCALE: | ||
381 | if (pValue.X < m_timestep) pValue.X = m_timestep; | ||
382 | if (pValue.Y < m_timestep) pValue.Y = m_timestep; | ||
383 | if (pValue.Z < m_timestep) pValue.Z = m_timestep; | ||
384 | |||
385 | m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
386 | break; | ||
387 | case Vehicle.ANGULAR_MOTOR_DIRECTION: | ||
388 | m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
389 | // Limit requested angular speed to 2 rps= 4 pi rads/sec | ||
390 | len = m_angularMotorDirection.Length(); | ||
391 | if (len > 12.566f) | ||
392 | m_angularMotorDirection *= (12.566f / len); | ||
393 | |||
394 | m_amEfect = 1.0f; // turn it on | ||
395 | m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; | ||
396 | |||
397 | if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) | ||
398 | && !rootPrim.m_isSelected && !rootPrim.m_disabled) | ||
399 | d.BodyEnable(rootPrim.Body); | ||
400 | break; | ||
401 | case Vehicle.LINEAR_FRICTION_TIMESCALE: | ||
402 | if (pValue.X < m_timestep) pValue.X = m_timestep; | ||
403 | if (pValue.Y < m_timestep) pValue.Y = m_timestep; | ||
404 | if (pValue.Z < m_timestep) pValue.Z = m_timestep; | ||
405 | m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
406 | break; | ||
407 | case Vehicle.LINEAR_MOTOR_DIRECTION: | ||
408 | m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
409 | len = m_linearMotorDirection.Length(); | ||
410 | if (len > 100.0f) | ||
411 | m_linearMotorDirection *= (100.0f / len); | ||
412 | |||
413 | m_lmEfect = 1.0f; // turn it on | ||
414 | m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale; | ||
415 | |||
416 | m_ffactor = 0.0f; | ||
417 | if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) | ||
418 | && !rootPrim.m_isSelected && !rootPrim.m_disabled) | ||
419 | d.BodyEnable(rootPrim.Body); | ||
420 | break; | ||
421 | case Vehicle.LINEAR_MOTOR_OFFSET: | ||
422 | m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
423 | len = m_linearMotorOffset.Length(); | ||
424 | if (len > 100.0f) | ||
425 | m_linearMotorOffset *= (100.0f / len); | ||
426 | break; | ||
427 | case Vehicle.BLOCK_EXIT: | ||
428 | m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
429 | break; | ||
430 | } | ||
431 | }//end ProcessVectorVehicleParam | ||
432 | |||
433 | internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) | ||
434 | { | ||
435 | switch (pParam) | ||
436 | { | ||
437 | case Vehicle.REFERENCE_FRAME: | ||
438 | // m_referenceFrame = Quaternion.Inverse(pValue); | ||
439 | m_referenceFrame = pValue; | ||
440 | break; | ||
441 | case Vehicle.ROLL_FRAME: | ||
442 | m_RollreferenceFrame = pValue; | ||
443 | break; | ||
444 | } | ||
445 | }//end ProcessRotationVehicleParam | ||
446 | |||
447 | internal void ProcessVehicleFlags(int pParam, bool remove) | ||
448 | { | ||
449 | if (remove) | ||
450 | { | ||
451 | m_flags &= ~((VehicleFlag)pParam); | ||
452 | } | ||
453 | else | ||
454 | { | ||
455 | m_flags |= (VehicleFlag)pParam; | ||
456 | } | ||
457 | }//end ProcessVehicleFlags | ||
458 | |||
459 | internal void ProcessTypeChange(Vehicle pType) | ||
460 | { | ||
461 | m_lmEfect = 0; | ||
462 | |||
463 | m_amEfect = 0; | ||
464 | m_ffactor = 1f; | ||
465 | |||
466 | m_linearMotorDirection = Vector3.Zero; | ||
467 | m_angularMotorDirection = Vector3.Zero; | ||
468 | |||
469 | m_BlockingEndPoint = Vector3.Zero; | ||
470 | m_RollreferenceFrame = Quaternion.Identity; | ||
471 | m_linearMotorOffset = Vector3.Zero; | ||
472 | |||
473 | m_referenceFrame = Quaternion.Identity; | ||
474 | |||
475 | // Set Defaults For Type | ||
476 | m_type = pType; | ||
477 | switch (pType) | ||
478 | { | ||
479 | case Vehicle.TYPE_NONE: | ||
480 | m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); | ||
481 | m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); | ||
482 | m_linearMotorTimescale = 1000; | ||
483 | m_linearMotorDecayTimescale = 120 * m_invtimestep; | ||
484 | m_angularMotorTimescale = 1000; | ||
485 | m_angularMotorDecayTimescale = 1000 * m_invtimestep; | ||
486 | m_VhoverHeight = 0; | ||
487 | m_VhoverEfficiency = 1; | ||
488 | m_VhoverTimescale = 1000; | ||
489 | m_VehicleBuoyancy = 0; | ||
490 | m_linearDeflectionEfficiency = 0; | ||
491 | m_linearDeflectionTimescale = 1000; | ||
492 | m_angularDeflectionEfficiency = 0; | ||
493 | m_angularDeflectionTimescale = 1000; | ||
494 | m_bankingEfficiency = 0; | ||
495 | m_bankingMix = 1; | ||
496 | m_bankingTimescale = 1000; | ||
497 | m_verticalAttractionEfficiency = 0; | ||
498 | m_verticalAttractionTimescale = 1000; | ||
499 | |||
500 | m_flags = (VehicleFlag)0; | ||
501 | break; | ||
502 | |||
503 | case Vehicle.TYPE_SLED: | ||
504 | m_linearFrictionTimescale = new Vector3(30, 1, 1000); | ||
505 | m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); | ||
506 | m_linearMotorTimescale = 1000; | ||
507 | m_linearMotorDecayTimescale = 120 * m_invtimestep; | ||
508 | m_angularMotorTimescale = 1000; | ||
509 | m_angularMotorDecayTimescale = 120 * m_invtimestep; | ||
510 | m_VhoverHeight = 0; | ||
511 | m_VhoverEfficiency = 1; | ||
512 | m_VhoverTimescale = 10; | ||
513 | m_VehicleBuoyancy = 0; | ||
514 | m_linearDeflectionEfficiency = 1; | ||
515 | m_linearDeflectionTimescale = 1; | ||
516 | m_angularDeflectionEfficiency = 0; | ||
517 | m_angularDeflectionTimescale = 10; | ||
518 | m_verticalAttractionEfficiency = 1; | ||
519 | m_verticalAttractionTimescale = 1000; | ||
520 | m_bankingEfficiency = 0; | ||
521 | m_bankingMix = 1; | ||
522 | m_bankingTimescale = 10; | ||
523 | m_flags &= | ||
524 | ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | | ||
525 | VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); | ||
526 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP | | ||
527 | VehicleFlag.LIMIT_ROLL_ONLY | | ||
528 | VehicleFlag.LIMIT_MOTOR_UP); | ||
529 | break; | ||
530 | |||
531 | case Vehicle.TYPE_CAR: | ||
532 | m_linearFrictionTimescale = new Vector3(100, 2, 1000); | ||
533 | m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); | ||
534 | m_linearMotorTimescale = 1; | ||
535 | m_linearMotorDecayTimescale = 60 * m_invtimestep; | ||
536 | m_angularMotorTimescale = 1; | ||
537 | m_angularMotorDecayTimescale = 0.8f * m_invtimestep; | ||
538 | m_VhoverHeight = 0; | ||
539 | m_VhoverEfficiency = 0; | ||
540 | m_VhoverTimescale = 1000; | ||
541 | m_VehicleBuoyancy = 0; | ||
542 | m_linearDeflectionEfficiency = 1; | ||
543 | m_linearDeflectionTimescale = 2; | ||
544 | m_angularDeflectionEfficiency = 0; | ||
545 | m_angularDeflectionTimescale = 10; | ||
546 | m_verticalAttractionEfficiency = 1f; | ||
547 | m_verticalAttractionTimescale = 10f; | ||
548 | m_bankingEfficiency = -0.2f; | ||
549 | m_bankingMix = 1; | ||
550 | m_bankingTimescale = 1; | ||
551 | m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | | ||
552 | VehicleFlag.HOVER_TERRAIN_ONLY | | ||
553 | VehicleFlag.HOVER_GLOBAL_HEIGHT); | ||
554 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP | | ||
555 | VehicleFlag.LIMIT_ROLL_ONLY | | ||
556 | VehicleFlag.LIMIT_MOTOR_UP | | ||
557 | VehicleFlag.HOVER_UP_ONLY); | ||
558 | break; | ||
559 | case Vehicle.TYPE_BOAT: | ||
560 | m_linearFrictionTimescale = new Vector3(10, 3, 2); | ||
561 | m_angularFrictionTimescale = new Vector3(10, 10, 10); | ||
562 | m_linearMotorTimescale = 5; | ||
563 | m_linearMotorDecayTimescale = 60 * m_invtimestep; | ||
564 | m_angularMotorTimescale = 4; | ||
565 | m_angularMotorDecayTimescale = 4 * m_invtimestep; | ||
566 | m_VhoverHeight = 0; | ||
567 | m_VhoverEfficiency = 0.5f; | ||
568 | m_VhoverTimescale = 2; | ||
569 | m_VehicleBuoyancy = 1; | ||
570 | m_linearDeflectionEfficiency = 0.5f; | ||
571 | m_linearDeflectionTimescale = 3; | ||
572 | m_angularDeflectionEfficiency = 0.5f; | ||
573 | m_angularDeflectionTimescale = 5; | ||
574 | m_verticalAttractionEfficiency = 0.5f; | ||
575 | m_verticalAttractionTimescale = 5f; | ||
576 | m_bankingEfficiency = -0.3f; | ||
577 | m_bankingMix = 0.8f; | ||
578 | m_bankingTimescale = 1; | ||
579 | m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | | ||
580 | VehicleFlag.HOVER_GLOBAL_HEIGHT | | ||
581 | VehicleFlag.HOVER_UP_ONLY); // | | ||
582 | // VehicleFlag.LIMIT_ROLL_ONLY); | ||
583 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP | | ||
584 | VehicleFlag.LIMIT_MOTOR_UP | | ||
585 | VehicleFlag.HOVER_UP_ONLY | // new sl | ||
586 | VehicleFlag.HOVER_WATER_ONLY); | ||
587 | break; | ||
588 | |||
589 | case Vehicle.TYPE_AIRPLANE: | ||
590 | m_linearFrictionTimescale = new Vector3(200, 10, 5); | ||
591 | m_angularFrictionTimescale = new Vector3(20, 20, 20); | ||
592 | m_linearMotorTimescale = 2; | ||
593 | m_linearMotorDecayTimescale = 60 * m_invtimestep; | ||
594 | m_angularMotorTimescale = 4; | ||
595 | m_angularMotorDecayTimescale = 8 * m_invtimestep; | ||
596 | m_VhoverHeight = 0; | ||
597 | m_VhoverEfficiency = 0.5f; | ||
598 | m_VhoverTimescale = 1000; | ||
599 | m_VehicleBuoyancy = 0; | ||
600 | m_linearDeflectionEfficiency = 0.5f; | ||
601 | m_linearDeflectionTimescale = 0.5f; | ||
602 | m_angularDeflectionEfficiency = 1; | ||
603 | m_angularDeflectionTimescale = 2; | ||
604 | m_verticalAttractionEfficiency = 0.9f; | ||
605 | m_verticalAttractionTimescale = 2f; | ||
606 | m_bankingEfficiency = 1; | ||
607 | m_bankingMix = 0.7f; | ||
608 | m_bankingTimescale = 2; | ||
609 | m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | | ||
610 | VehicleFlag.HOVER_TERRAIN_ONLY | | ||
611 | VehicleFlag.HOVER_GLOBAL_HEIGHT | | ||
612 | VehicleFlag.HOVER_UP_ONLY | | ||
613 | VehicleFlag.NO_DEFLECTION_UP | | ||
614 | VehicleFlag.LIMIT_MOTOR_UP); | ||
615 | m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); | ||
616 | break; | ||
617 | |||
618 | case Vehicle.TYPE_BALLOON: | ||
619 | m_linearFrictionTimescale = new Vector3(5, 5, 5); | ||
620 | m_angularFrictionTimescale = new Vector3(10, 10, 10); | ||
621 | m_linearMotorTimescale = 5; | ||
622 | m_linearMotorDecayTimescale = 60 * m_invtimestep; | ||
623 | m_angularMotorTimescale = 6; | ||
624 | m_angularMotorDecayTimescale = 10 * m_invtimestep; | ||
625 | m_VhoverHeight = 5; | ||
626 | m_VhoverEfficiency = 0.8f; | ||
627 | m_VhoverTimescale = 10; | ||
628 | m_VehicleBuoyancy = 1; | ||
629 | m_linearDeflectionEfficiency = 0; | ||
630 | m_linearDeflectionTimescale = 5 * m_invtimestep; | ||
631 | m_angularDeflectionEfficiency = 0; | ||
632 | m_angularDeflectionTimescale = 5; | ||
633 | m_verticalAttractionEfficiency = 1f; | ||
634 | m_verticalAttractionTimescale = 1000f; | ||
635 | m_bankingEfficiency = 0; | ||
636 | m_bankingMix = 0.7f; | ||
637 | m_bankingTimescale = 5; | ||
638 | m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | | ||
639 | VehicleFlag.HOVER_TERRAIN_ONLY | | ||
640 | VehicleFlag.HOVER_UP_ONLY | | ||
641 | VehicleFlag.NO_DEFLECTION_UP | | ||
642 | VehicleFlag.LIMIT_MOTOR_UP | //); | ||
643 | VehicleFlag.LIMIT_ROLL_ONLY | // new sl | ||
644 | VehicleFlag.HOVER_GLOBAL_HEIGHT); // new sl | ||
645 | |||
646 | // m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | | ||
647 | // VehicleFlag.HOVER_GLOBAL_HEIGHT); | ||
648 | break; | ||
649 | |||
650 | } | ||
651 | |||
652 | m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale); | ||
653 | m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; | ||
654 | |||
655 | }//end SetDefaultsForType | ||
656 | |||
657 | internal void Stop() | ||
658 | { | ||
659 | m_lmEfect = 0; | ||
660 | m_lmDecay = 0f; | ||
661 | m_amEfect = 0; | ||
662 | m_amDecay = 0; | ||
663 | m_ffactor = 1f; | ||
664 | } | ||
665 | |||
666 | public static Vector3 Xrot(Quaternion rot) | ||
667 | { | ||
668 | Vector3 vec; | ||
669 | rot.Normalize(); // just in case | ||
670 | vec.X = 2 * (rot.X * rot.X + rot.W * rot.W) - 1; | ||
671 | vec.Y = 2 * (rot.X * rot.Y + rot.Z * rot.W); | ||
672 | vec.Z = 2 * (rot.X * rot.Z - rot.Y * rot.W); | ||
673 | return vec; | ||
674 | } | ||
675 | |||
676 | public static Vector3 Zrot(Quaternion rot) | ||
677 | { | ||
678 | Vector3 vec; | ||
679 | rot.Normalize(); // just in case | ||
680 | vec.X = 2 * (rot.X * rot.Z + rot.Y * rot.W); | ||
681 | vec.Y = 2 * (rot.Y * rot.Z - rot.X * rot.W); | ||
682 | vec.Z = 2 * (rot.Z * rot.Z + rot.W * rot.W) - 1; | ||
683 | |||
684 | return vec; | ||
685 | } | ||
686 | |||
687 | private const float pi = (float)Math.PI; | ||
688 | private const float halfpi = 0.5f * (float)Math.PI; | ||
689 | private const float twopi = 2.0f * pi; | ||
690 | |||
691 | public static Vector3 ubRot2Euler(Quaternion rot) | ||
692 | { | ||
693 | // returns roll in X | ||
694 | // pitch in Y | ||
695 | // yaw in Z | ||
696 | Vector3 vec; | ||
697 | |||
698 | // assuming rot is normalised | ||
699 | // rot.Normalize(); | ||
700 | |||
701 | float zX = rot.X * rot.Z + rot.Y * rot.W; | ||
702 | |||
703 | if (zX < -0.49999f) | ||
704 | { | ||
705 | vec.X = 0; | ||
706 | vec.Y = -halfpi; | ||
707 | vec.Z = (float)(-2d * Math.Atan(rot.X / rot.W)); | ||
708 | } | ||
709 | else if (zX > 0.49999f) | ||
710 | { | ||
711 | vec.X = 0; | ||
712 | vec.Y = halfpi; | ||
713 | vec.Z = (float)(2d * Math.Atan(rot.X / rot.W)); | ||
714 | } | ||
715 | else | ||
716 | { | ||
717 | vec.Y = (float)Math.Asin(2 * zX); | ||
718 | |||
719 | float sqw = rot.W * rot.W; | ||
720 | |||
721 | float minuszY = rot.X * rot.W - rot.Y * rot.Z; | ||
722 | float zZ = rot.Z * rot.Z + sqw - 0.5f; | ||
723 | |||
724 | vec.X = (float)Math.Atan2(minuszY, zZ); | ||
725 | |||
726 | float yX = rot.Z * rot.W - rot.X * rot.Y; //( have negative ?) | ||
727 | float yY = rot.X * rot.X + sqw - 0.5f; | ||
728 | vec.Z = (float)Math.Atan2(yX, yY); | ||
729 | } | ||
730 | return vec; | ||
731 | } | ||
732 | |||
733 | public static void GetRollPitch(Quaternion rot, out float roll, out float pitch) | ||
734 | { | ||
735 | // assuming rot is normalised | ||
736 | // rot.Normalize(); | ||
737 | |||
738 | float zX = rot.X * rot.Z + rot.Y * rot.W; | ||
739 | |||
740 | if (zX < -0.49999f) | ||
741 | { | ||
742 | roll = 0; | ||
743 | pitch = -halfpi; | ||
744 | } | ||
745 | else if (zX > 0.49999f) | ||
746 | { | ||
747 | roll = 0; | ||
748 | pitch = halfpi; | ||
749 | } | ||
750 | else | ||
751 | { | ||
752 | pitch = (float)Math.Asin(2 * zX); | ||
753 | |||
754 | float minuszY = rot.X * rot.W - rot.Y * rot.Z; | ||
755 | float zZ = rot.Z * rot.Z + rot.W * rot.W - 0.5f; | ||
756 | |||
757 | roll = (float)Math.Atan2(minuszY, zZ); | ||
758 | } | ||
759 | return ; | ||
760 | } | ||
761 | |||
762 | internal void Step() | ||
763 | { | ||
764 | IntPtr Body = rootPrim.Body; | ||
765 | |||
766 | d.Mass dmass; | ||
767 | d.BodyGetMass(Body, out dmass); | ||
768 | |||
769 | d.Quaternion rot = d.BodyGetQuaternion(Body); | ||
770 | Quaternion objrotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object | ||
771 | Quaternion rotq = objrotq; // rotq = rotation of object | ||
772 | rotq *= m_referenceFrame; // rotq is now rotation in vehicle reference frame | ||
773 | Quaternion irotq = Quaternion.Inverse(rotq); | ||
774 | |||
775 | d.Vector3 dvtmp; | ||
776 | Vector3 tmpV; | ||
777 | Vector3 curVel; // velocity in world | ||
778 | Vector3 curAngVel; // angular velocity in world | ||
779 | Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame | ||
780 | Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in vehicle frame | ||
781 | d.Vector3 dtorque = new d.Vector3(); | ||
782 | |||
783 | dvtmp = d.BodyGetLinearVel(Body); | ||
784 | curVel.X = dvtmp.X; | ||
785 | curVel.Y = dvtmp.Y; | ||
786 | curVel.Z = dvtmp.Z; | ||
787 | Vector3 curLocalVel = curVel * irotq; // current velocity in local | ||
788 | |||
789 | dvtmp = d.BodyGetAngularVel(Body); | ||
790 | curAngVel.X = dvtmp.X; | ||
791 | curAngVel.Y = dvtmp.Y; | ||
792 | curAngVel.Z = dvtmp.Z; | ||
793 | Vector3 curLocalAngVel = curAngVel * irotq; // current angular velocity in local | ||
794 | |||
795 | float ldampZ = 0; | ||
796 | |||
797 | // linear motor | ||
798 | if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000) | ||
799 | { | ||
800 | tmpV = m_linearMotorDirection - curLocalVel; // velocity error | ||
801 | tmpV *= m_lmEfect / m_linearMotorTimescale; // error to correct in this timestep | ||
802 | tmpV *= rotq; // to world | ||
803 | |||
804 | if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0) | ||
805 | tmpV.Z = 0; | ||
806 | |||
807 | if (m_linearMotorOffset.X != 0 || m_linearMotorOffset.Y != 0 || m_linearMotorOffset.Z != 0) | ||
808 | { | ||
809 | // have offset, do it now | ||
810 | tmpV *= dmass.mass; | ||
811 | d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z); | ||
812 | } | ||
813 | else | ||
814 | { | ||
815 | force.X += tmpV.X; | ||
816 | force.Y += tmpV.Y; | ||
817 | force.Z += tmpV.Z; | ||
818 | } | ||
819 | |||
820 | m_lmEfect *= m_lmDecay; | ||
821 | // m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared(); | ||
822 | m_ffactor = 0.0f; | ||
823 | } | ||
824 | else | ||
825 | { | ||
826 | m_lmEfect = 0; | ||
827 | m_ffactor = 1f; | ||
828 | } | ||
829 | |||
830 | // hover | ||
831 | if (m_VhoverTimescale < 300 && rootPrim.prim_geom != IntPtr.Zero) | ||
832 | { | ||
833 | // d.Vector3 pos = d.BodyGetPosition(Body); | ||
834 | d.Vector3 pos = d.GeomGetPosition(rootPrim.prim_geom); | ||
835 | pos.Z -= 0.21f; // minor offset that seems to be always there in sl | ||
836 | |||
837 | float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y); | ||
838 | float perr; | ||
839 | |||
840 | // default to global but don't go underground | ||
841 | perr = m_VhoverHeight - pos.Z; | ||
842 | |||
843 | if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0) | ||
844 | { | ||
845 | if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0) | ||
846 | { | ||
847 | perr += _pParentScene.GetWaterLevel(); | ||
848 | } | ||
849 | else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) | ||
850 | { | ||
851 | perr += t; | ||
852 | } | ||
853 | else | ||
854 | { | ||
855 | float w = _pParentScene.GetWaterLevel(); | ||
856 | if (t > w) | ||
857 | perr += t; | ||
858 | else | ||
859 | perr += w; | ||
860 | } | ||
861 | } | ||
862 | else if (t > m_VhoverHeight) | ||
863 | perr = t - pos.Z; ; | ||
864 | |||
865 | if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > -0.1) | ||
866 | { | ||
867 | ldampZ = m_VhoverEfficiency * m_invtimestep; | ||
868 | |||
869 | perr *= (1.0f + ldampZ) / m_VhoverTimescale; | ||
870 | |||
871 | // force.Z += perr - curVel.Z * tmp; | ||
872 | force.Z += perr; | ||
873 | ldampZ *= -curVel.Z; | ||
874 | |||
875 | force.Z += _pParentScene.gravityz * m_gravmod * (1f - m_VehicleBuoyancy); | ||
876 | } | ||
877 | else // no buoyancy | ||
878 | force.Z += _pParentScene.gravityz; | ||
879 | } | ||
880 | else | ||
881 | { | ||
882 | // default gravity and Buoyancy | ||
883 | force.Z += _pParentScene.gravityz * m_gravmod * (1f - m_VehicleBuoyancy); | ||
884 | } | ||
885 | |||
886 | // linear deflection | ||
887 | if (m_linearDeflectionEfficiency > 0) | ||
888 | { | ||
889 | float len = curVel.Length(); | ||
890 | if (len > 0.01) // if moving | ||
891 | { | ||
892 | Vector3 atAxis; | ||
893 | atAxis = Xrot(rotq); // where are we pointing to | ||
894 | atAxis *= len; // make it same size as world velocity vector | ||
895 | |||
896 | tmpV = -atAxis; // oposite direction | ||
897 | atAxis -= curVel; // error to one direction | ||
898 | len = atAxis.LengthSquared(); | ||
899 | |||
900 | tmpV -= curVel; // error to oposite | ||
901 | float lens = tmpV.LengthSquared(); | ||
902 | |||
903 | if (len > 0.01 || lens > 0.01) // do nothing if close enougth | ||
904 | { | ||
905 | if (len < lens) | ||
906 | tmpV = atAxis; | ||
907 | |||
908 | tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep | ||
909 | force.X += tmpV.X; | ||
910 | force.Y += tmpV.Y; | ||
911 | if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0) | ||
912 | force.Z += tmpV.Z; | ||
913 | } | ||
914 | } | ||
915 | } | ||
916 | |||
917 | // linear friction/damping | ||
918 | if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0) | ||
919 | { | ||
920 | tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X; | ||
921 | tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y; | ||
922 | tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z; | ||
923 | tmpV *= rotq; // to world | ||
924 | |||
925 | if(ldampZ != 0 && Math.Abs(ldampZ) > Math.Abs(tmpV.Z)) | ||
926 | tmpV.Z = ldampZ; | ||
927 | force.X += tmpV.X; | ||
928 | force.Y += tmpV.Y; | ||
929 | force.Z += tmpV.Z; | ||
930 | } | ||
931 | |||
932 | // vertical atractor | ||
933 | if (m_verticalAttractionTimescale < 300) | ||
934 | { | ||
935 | float roll; | ||
936 | float pitch; | ||
937 | |||
938 | |||
939 | |||
940 | float ftmp = m_invtimestep / m_verticalAttractionTimescale / m_verticalAttractionTimescale; | ||
941 | |||
942 | float ftmp2; | ||
943 | ftmp2 = 0.5f * m_verticalAttractionEfficiency * m_invtimestep; | ||
944 | m_amdampX = ftmp2; | ||
945 | |||
946 | m_ampwr = 1.0f - 0.8f * m_verticalAttractionEfficiency; | ||
947 | |||
948 | GetRollPitch(irotq, out roll, out pitch); | ||
949 | |||
950 | if (roll > halfpi) | ||
951 | roll = pi - roll; | ||
952 | else if (roll < -halfpi) | ||
953 | roll = -pi - roll; | ||
954 | |||
955 | float effroll = pitch / halfpi; | ||
956 | effroll *= effroll; | ||
957 | effroll = 1 - effroll; | ||
958 | effroll *= roll; | ||
959 | |||
960 | |||
961 | torque.X += effroll * ftmp; | ||
962 | |||
963 | if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0) | ||
964 | { | ||
965 | float effpitch = roll / halfpi; | ||
966 | effpitch *= effpitch; | ||
967 | effpitch = 1 - effpitch; | ||
968 | effpitch *= pitch; | ||
969 | |||
970 | torque.Y += effpitch * ftmp; | ||
971 | } | ||
972 | |||
973 | if (m_bankingEfficiency != 0 && Math.Abs(effroll) > 0.01) | ||
974 | { | ||
975 | |||
976 | float broll = effroll; | ||
977 | /* | ||
978 | if (broll > halfpi) | ||
979 | broll = pi - broll; | ||
980 | else if (broll < -halfpi) | ||
981 | broll = -pi - broll; | ||
982 | */ | ||
983 | broll *= m_bankingEfficiency; | ||
984 | if (m_bankingMix != 0) | ||
985 | { | ||
986 | float vfact = Math.Abs(curLocalVel.X) / 10.0f; | ||
987 | if (vfact > 1.0f) vfact = 1.0f; | ||
988 | |||
989 | if (curLocalVel.X >= 0) | ||
990 | broll *= (1 + (vfact - 1) * m_bankingMix); | ||
991 | else | ||
992 | broll *= -(1 + (vfact - 1) * m_bankingMix); | ||
993 | } | ||
994 | // make z rot be in world Z not local as seems to be in sl | ||
995 | |||
996 | broll = broll / m_bankingTimescale; | ||
997 | |||
998 | |||
999 | tmpV = Zrot(irotq); | ||
1000 | tmpV *= broll; | ||
1001 | |||
1002 | torque.X += tmpV.X; | ||
1003 | torque.Y += tmpV.Y; | ||
1004 | torque.Z += tmpV.Z; | ||
1005 | |||
1006 | m_amdampZ = Math.Abs(m_bankingEfficiency) / m_bankingTimescale; | ||
1007 | m_amdampY = m_amdampZ; | ||
1008 | |||
1009 | } | ||
1010 | else | ||
1011 | { | ||
1012 | m_amdampZ = 1 / m_angularFrictionTimescale.Z; | ||
1013 | m_amdampY = m_amdampX; | ||
1014 | } | ||
1015 | } | ||
1016 | else | ||
1017 | { | ||
1018 | m_ampwr = 1.0f; | ||
1019 | m_amdampX = 1 / m_angularFrictionTimescale.X; | ||
1020 | m_amdampY = 1 / m_angularFrictionTimescale.Y; | ||
1021 | m_amdampZ = 1 / m_angularFrictionTimescale.Z; | ||
1022 | } | ||
1023 | |||
1024 | // angular motor | ||
1025 | if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000) | ||
1026 | { | ||
1027 | tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error | ||
1028 | tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep | ||
1029 | torque.X += tmpV.X * m_ampwr; | ||
1030 | torque.Y += tmpV.Y * m_ampwr; | ||
1031 | torque.Z += tmpV.Z; | ||
1032 | |||
1033 | m_amEfect *= m_amDecay; | ||
1034 | } | ||
1035 | else | ||
1036 | m_amEfect = 0; | ||
1037 | |||
1038 | // angular deflection | ||
1039 | if (m_angularDeflectionEfficiency > 0) | ||
1040 | { | ||
1041 | Vector3 dirv; | ||
1042 | |||
1043 | if (curLocalVel.X > 0.01f) | ||
1044 | dirv = curLocalVel; | ||
1045 | else if (curLocalVel.X < -0.01f) | ||
1046 | // use oposite | ||
1047 | dirv = -curLocalVel; | ||
1048 | else | ||
1049 | { | ||
1050 | // make it fall into small positive x case | ||
1051 | dirv.X = 0.01f; | ||
1052 | dirv.Y = curLocalVel.Y; | ||
1053 | dirv.Z = curLocalVel.Z; | ||
1054 | } | ||
1055 | |||
1056 | float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale; | ||
1057 | |||
1058 | if (Math.Abs(dirv.Z) > 0.01) | ||
1059 | { | ||
1060 | torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp; | ||
1061 | } | ||
1062 | |||
1063 | if (Math.Abs(dirv.Y) > 0.01) | ||
1064 | { | ||
1065 | torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp; | ||
1066 | } | ||
1067 | } | ||
1068 | |||
1069 | // angular friction | ||
1070 | if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0) | ||
1071 | { | ||
1072 | torque.X -= curLocalAngVel.X * m_amdampX; | ||
1073 | torque.Y -= curLocalAngVel.Y * m_amdampY; | ||
1074 | torque.Z -= curLocalAngVel.Z * m_amdampZ; | ||
1075 | } | ||
1076 | |||
1077 | |||
1078 | if (force.X != 0 || force.Y != 0 || force.Z != 0) | ||
1079 | { | ||
1080 | force *= dmass.mass; | ||
1081 | d.BodyAddForce(Body, force.X, force.Y, force.Z); | ||
1082 | } | ||
1083 | |||
1084 | if (torque.X != 0 || torque.Y != 0 || torque.Z != 0) | ||
1085 | { | ||
1086 | torque *= m_referenceFrame; // to object frame | ||
1087 | dtorque.X = torque.X ; | ||
1088 | dtorque.Y = torque.Y; | ||
1089 | dtorque.Z = torque.Z; | ||
1090 | |||
1091 | d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque); | ||
1092 | d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame | ||
1093 | } | ||
1094 | } | ||
1095 | } | ||
1096 | } | ||