diff options
Diffstat (limited to '')
-rw-r--r-- | OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs | 1086 |
1 files changed, 1086 insertions, 0 deletions
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs new file mode 100644 index 0000000..e900c02 --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs | |||
@@ -0,0 +1,1086 @@ | |||
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.Physics.Manager; | ||
52 | |||
53 | namespace OpenSim.Region.Physics.OdePlugin | ||
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 | |||
141 | public float FrictionFactor | ||
142 | { | ||
143 | get | ||
144 | { | ||
145 | return m_ffactor; | ||
146 | } | ||
147 | } | ||
148 | |||
149 | |||
150 | public ODEDynamics(OdePrim rootp) | ||
151 | { | ||
152 | rootPrim = rootp; | ||
153 | _pParentScene = rootPrim._parent_scene; | ||
154 | m_timestep = _pParentScene.ODE_STEPSIZE; | ||
155 | m_invtimestep = 1.0f / m_timestep; | ||
156 | } | ||
157 | |||
158 | public void DoSetVehicle(VehicleData vd) | ||
159 | { | ||
160 | m_type = vd.m_type; | ||
161 | m_flags = vd.m_flags; | ||
162 | |||
163 | |||
164 | // Linear properties | ||
165 | m_linearMotorDirection = vd.m_linearMotorDirection; | ||
166 | |||
167 | m_linearFrictionTimescale = vd.m_linearFrictionTimescale; | ||
168 | if (m_linearFrictionTimescale.X < m_timestep) m_linearFrictionTimescale.X = m_timestep; | ||
169 | if (m_linearFrictionTimescale.Y < m_timestep) m_linearFrictionTimescale.Y = m_timestep; | ||
170 | if (m_linearFrictionTimescale.Z < m_timestep) m_linearFrictionTimescale.Z = m_timestep; | ||
171 | |||
172 | m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale; | ||
173 | if (m_linearMotorDecayTimescale < m_timestep) m_linearMotorDecayTimescale = m_timestep; | ||
174 | m_linearMotorDecayTimescale += 0.2f; | ||
175 | m_linearMotorDecayTimescale *= m_invtimestep; | ||
176 | |||
177 | m_linearMotorTimescale = vd.m_linearMotorTimescale; | ||
178 | if (m_linearMotorTimescale < m_timestep) m_linearMotorTimescale = m_timestep; | ||
179 | |||
180 | m_linearMotorOffset = vd.m_linearMotorOffset; | ||
181 | |||
182 | //Angular properties | ||
183 | m_angularMotorDirection = vd.m_angularMotorDirection; | ||
184 | m_angularMotorTimescale = vd.m_angularMotorTimescale; | ||
185 | if (m_angularMotorTimescale < m_timestep) m_angularMotorTimescale = m_timestep; | ||
186 | |||
187 | m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale; | ||
188 | if (m_angularMotorDecayTimescale < m_timestep) m_angularMotorDecayTimescale = m_timestep; | ||
189 | m_angularMotorDecayTimescale *= m_invtimestep; | ||
190 | |||
191 | m_angularFrictionTimescale = vd.m_angularFrictionTimescale; | ||
192 | if (m_angularFrictionTimescale.X < m_timestep) m_angularFrictionTimescale.X = m_timestep; | ||
193 | if (m_angularFrictionTimescale.Y < m_timestep) m_angularFrictionTimescale.Y = m_timestep; | ||
194 | if (m_angularFrictionTimescale.Z < m_timestep) m_angularFrictionTimescale.Z = m_timestep; | ||
195 | |||
196 | //Deflection properties | ||
197 | m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency; | ||
198 | m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale; | ||
199 | if (m_angularDeflectionTimescale < m_timestep) m_angularDeflectionTimescale = m_timestep; | ||
200 | |||
201 | m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency; | ||
202 | m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale; | ||
203 | if (m_linearDeflectionTimescale < m_timestep) m_linearDeflectionTimescale = m_timestep; | ||
204 | |||
205 | //Banking properties | ||
206 | m_bankingEfficiency = vd.m_bankingEfficiency; | ||
207 | m_bankingMix = vd.m_bankingMix; | ||
208 | m_bankingTimescale = vd.m_bankingTimescale; | ||
209 | if (m_bankingTimescale < m_timestep) m_bankingTimescale = m_timestep; | ||
210 | |||
211 | //Hover and Buoyancy properties | ||
212 | m_VhoverHeight = vd.m_VhoverHeight; | ||
213 | m_VhoverEfficiency = vd.m_VhoverEfficiency; | ||
214 | m_VhoverTimescale = vd.m_VhoverTimescale; | ||
215 | if (m_VhoverTimescale < m_timestep) m_VhoverTimescale = m_timestep; | ||
216 | |||
217 | m_VehicleBuoyancy = vd.m_VehicleBuoyancy; | ||
218 | |||
219 | //Attractor properties | ||
220 | m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency; | ||
221 | m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale; | ||
222 | if (m_verticalAttractionTimescale < m_timestep) m_verticalAttractionTimescale = m_timestep; | ||
223 | |||
224 | // Axis | ||
225 | m_referenceFrame = vd.m_referenceFrame; | ||
226 | |||
227 | m_lmEfect = 0; | ||
228 | m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale); | ||
229 | m_amEfect = 0; | ||
230 | m_ffactor = 1.0f; | ||
231 | } | ||
232 | |||
233 | internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) | ||
234 | { | ||
235 | float len; | ||
236 | |||
237 | switch (pParam) | ||
238 | { | ||
239 | case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY: | ||
240 | if (pValue < 0f) pValue = 0f; | ||
241 | if (pValue > 1f) pValue = 1f; | ||
242 | m_angularDeflectionEfficiency = pValue; | ||
243 | break; | ||
244 | case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: | ||
245 | if (pValue < m_timestep) pValue = m_timestep; | ||
246 | m_angularDeflectionTimescale = pValue; | ||
247 | break; | ||
248 | case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: | ||
249 | if (pValue < m_timestep) pValue = m_timestep; | ||
250 | else if (pValue > 120) pValue = 120; | ||
251 | m_angularMotorDecayTimescale = pValue * m_invtimestep; | ||
252 | m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; | ||
253 | break; | ||
254 | case Vehicle.ANGULAR_MOTOR_TIMESCALE: | ||
255 | if (pValue < m_timestep) pValue = m_timestep; | ||
256 | m_angularMotorTimescale = pValue; | ||
257 | break; | ||
258 | case Vehicle.BANKING_EFFICIENCY: | ||
259 | if (pValue < -1f) pValue = -1f; | ||
260 | if (pValue > 1f) pValue = 1f; | ||
261 | m_bankingEfficiency = pValue; | ||
262 | break; | ||
263 | case Vehicle.BANKING_MIX: | ||
264 | if (pValue < 0f) pValue = 0f; | ||
265 | if (pValue > 1f) pValue = 1f; | ||
266 | m_bankingMix = pValue; | ||
267 | break; | ||
268 | case Vehicle.BANKING_TIMESCALE: | ||
269 | if (pValue < m_timestep) pValue = m_timestep; | ||
270 | m_bankingTimescale = pValue; | ||
271 | break; | ||
272 | case Vehicle.BUOYANCY: | ||
273 | if (pValue < -1f) pValue = -1f; | ||
274 | if (pValue > 1f) pValue = 1f; | ||
275 | m_VehicleBuoyancy = pValue; | ||
276 | break; | ||
277 | case Vehicle.HOVER_EFFICIENCY: | ||
278 | if (pValue < 0f) pValue = 0f; | ||
279 | if (pValue > 1f) pValue = 1f; | ||
280 | m_VhoverEfficiency = pValue; | ||
281 | break; | ||
282 | case Vehicle.HOVER_HEIGHT: | ||
283 | m_VhoverHeight = pValue; | ||
284 | break; | ||
285 | case Vehicle.HOVER_TIMESCALE: | ||
286 | if (pValue < m_timestep) pValue = m_timestep; | ||
287 | m_VhoverTimescale = pValue; | ||
288 | break; | ||
289 | case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: | ||
290 | if (pValue < 0f) pValue = 0f; | ||
291 | if (pValue > 1f) pValue = 1f; | ||
292 | m_linearDeflectionEfficiency = pValue; | ||
293 | break; | ||
294 | case Vehicle.LINEAR_DEFLECTION_TIMESCALE: | ||
295 | if (pValue < m_timestep) pValue = m_timestep; | ||
296 | m_linearDeflectionTimescale = pValue; | ||
297 | break; | ||
298 | case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: | ||
299 | if (pValue < m_timestep) pValue = m_timestep; | ||
300 | else if (pValue > 120) pValue = 120; | ||
301 | m_linearMotorDecayTimescale = (0.2f +pValue) * m_invtimestep; | ||
302 | m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale); | ||
303 | break; | ||
304 | case Vehicle.LINEAR_MOTOR_TIMESCALE: | ||
305 | if (pValue < m_timestep) pValue = m_timestep; | ||
306 | m_linearMotorTimescale = pValue; | ||
307 | break; | ||
308 | case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: | ||
309 | if (pValue < 0f) pValue = 0f; | ||
310 | if (pValue > 1f) pValue = 1f; | ||
311 | m_verticalAttractionEfficiency = pValue; | ||
312 | break; | ||
313 | case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: | ||
314 | if (pValue < m_timestep) pValue = m_timestep; | ||
315 | m_verticalAttractionTimescale = pValue; | ||
316 | break; | ||
317 | |||
318 | // These are vector properties but the engine lets you use a single float value to | ||
319 | // set all of the components to the same value | ||
320 | case Vehicle.ANGULAR_FRICTION_TIMESCALE: | ||
321 | if (pValue < m_timestep) pValue = m_timestep; | ||
322 | m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); | ||
323 | break; | ||
324 | case Vehicle.ANGULAR_MOTOR_DIRECTION: | ||
325 | m_angularMotorDirection = new Vector3(pValue, pValue, pValue); | ||
326 | len = m_angularMotorDirection.Length(); | ||
327 | if (len > 12.566f) | ||
328 | m_angularMotorDirection *= (12.566f / len); | ||
329 | |||
330 | m_amEfect = 1.0f ; // turn it on | ||
331 | m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; | ||
332 | |||
333 | if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) | ||
334 | && !rootPrim.m_isSelected && !rootPrim.m_disabled) | ||
335 | d.BodyEnable(rootPrim.Body); | ||
336 | break; | ||
337 | case Vehicle.LINEAR_FRICTION_TIMESCALE: | ||
338 | if (pValue < m_timestep) pValue = m_timestep; | ||
339 | m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); | ||
340 | break; | ||
341 | case Vehicle.LINEAR_MOTOR_DIRECTION: | ||
342 | m_linearMotorDirection = new Vector3(pValue, pValue, pValue); | ||
343 | len = m_linearMotorDirection.Length(); | ||
344 | if (len > 100.0f) | ||
345 | m_linearMotorDirection *= (100.0f / len); | ||
346 | |||
347 | m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale; | ||
348 | m_lmEfect = 1.0f; // turn it on | ||
349 | |||
350 | m_ffactor = 0.0f; | ||
351 | if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) | ||
352 | && !rootPrim.m_isSelected && !rootPrim.m_disabled) | ||
353 | d.BodyEnable(rootPrim.Body); | ||
354 | break; | ||
355 | case Vehicle.LINEAR_MOTOR_OFFSET: | ||
356 | m_linearMotorOffset = new Vector3(pValue, pValue, pValue); | ||
357 | len = m_linearMotorOffset.Length(); | ||
358 | if (len > 100.0f) | ||
359 | m_linearMotorOffset *= (100.0f / len); | ||
360 | break; | ||
361 | } | ||
362 | }//end ProcessFloatVehicleParam | ||
363 | |||
364 | internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) | ||
365 | { | ||
366 | float len; | ||
367 | |||
368 | switch (pParam) | ||
369 | { | ||
370 | case Vehicle.ANGULAR_FRICTION_TIMESCALE: | ||
371 | if (pValue.X < m_timestep) pValue.X = m_timestep; | ||
372 | if (pValue.Y < m_timestep) pValue.Y = m_timestep; | ||
373 | if (pValue.Z < m_timestep) pValue.Z = m_timestep; | ||
374 | |||
375 | m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
376 | break; | ||
377 | case Vehicle.ANGULAR_MOTOR_DIRECTION: | ||
378 | m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
379 | // Limit requested angular speed to 2 rps= 4 pi rads/sec | ||
380 | len = m_angularMotorDirection.Length(); | ||
381 | if (len > 12.566f) | ||
382 | m_angularMotorDirection *= (12.566f / len); | ||
383 | |||
384 | m_amEfect = 1.0f; // turn it on | ||
385 | m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; | ||
386 | |||
387 | if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) | ||
388 | && !rootPrim.m_isSelected && !rootPrim.m_disabled) | ||
389 | d.BodyEnable(rootPrim.Body); | ||
390 | break; | ||
391 | case Vehicle.LINEAR_FRICTION_TIMESCALE: | ||
392 | if (pValue.X < m_timestep) pValue.X = m_timestep; | ||
393 | if (pValue.Y < m_timestep) pValue.Y = m_timestep; | ||
394 | if (pValue.Z < m_timestep) pValue.Z = m_timestep; | ||
395 | m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
396 | break; | ||
397 | case Vehicle.LINEAR_MOTOR_DIRECTION: | ||
398 | m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
399 | len = m_linearMotorDirection.Length(); | ||
400 | if (len > 100.0f) | ||
401 | m_linearMotorDirection *= (100.0f / len); | ||
402 | |||
403 | m_lmEfect = 1.0f; // turn it on | ||
404 | m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale; | ||
405 | |||
406 | m_ffactor = 0.0f; | ||
407 | if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) | ||
408 | && !rootPrim.m_isSelected && !rootPrim.m_disabled) | ||
409 | d.BodyEnable(rootPrim.Body); | ||
410 | break; | ||
411 | case Vehicle.LINEAR_MOTOR_OFFSET: | ||
412 | m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
413 | len = m_linearMotorOffset.Length(); | ||
414 | if (len > 100.0f) | ||
415 | m_linearMotorOffset *= (100.0f / len); | ||
416 | break; | ||
417 | case Vehicle.BLOCK_EXIT: | ||
418 | m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
419 | break; | ||
420 | } | ||
421 | }//end ProcessVectorVehicleParam | ||
422 | |||
423 | internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) | ||
424 | { | ||
425 | switch (pParam) | ||
426 | { | ||
427 | case Vehicle.REFERENCE_FRAME: | ||
428 | m_referenceFrame = Quaternion.Inverse(pValue); | ||
429 | break; | ||
430 | case Vehicle.ROLL_FRAME: | ||
431 | m_RollreferenceFrame = pValue; | ||
432 | break; | ||
433 | } | ||
434 | }//end ProcessRotationVehicleParam | ||
435 | |||
436 | internal void ProcessVehicleFlags(int pParam, bool remove) | ||
437 | { | ||
438 | if (remove) | ||
439 | { | ||
440 | m_flags &= ~((VehicleFlag)pParam); | ||
441 | } | ||
442 | else | ||
443 | { | ||
444 | m_flags |= (VehicleFlag)pParam; | ||
445 | } | ||
446 | }//end ProcessVehicleFlags | ||
447 | |||
448 | internal void ProcessTypeChange(Vehicle pType) | ||
449 | { | ||
450 | m_lmEfect = 0; | ||
451 | |||
452 | m_amEfect = 0; | ||
453 | m_ffactor = 1f; | ||
454 | |||
455 | m_linearMotorDirection = Vector3.Zero; | ||
456 | m_angularMotorDirection = Vector3.Zero; | ||
457 | |||
458 | m_BlockingEndPoint = Vector3.Zero; | ||
459 | m_RollreferenceFrame = Quaternion.Identity; | ||
460 | m_linearMotorOffset = Vector3.Zero; | ||
461 | |||
462 | m_referenceFrame = Quaternion.Identity; | ||
463 | |||
464 | // Set Defaults For Type | ||
465 | m_type = pType; | ||
466 | switch (pType) | ||
467 | { | ||
468 | case Vehicle.TYPE_NONE: | ||
469 | m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); | ||
470 | m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); | ||
471 | m_linearMotorTimescale = 1000; | ||
472 | m_linearMotorDecayTimescale = 120 * m_invtimestep; | ||
473 | m_angularMotorTimescale = 1000; | ||
474 | m_angularMotorDecayTimescale = 1000 * m_invtimestep; | ||
475 | m_VhoverHeight = 0; | ||
476 | m_VhoverEfficiency = 1; | ||
477 | m_VhoverTimescale = 1000; | ||
478 | m_VehicleBuoyancy = 0; | ||
479 | m_linearDeflectionEfficiency = 0; | ||
480 | m_linearDeflectionTimescale = 1000; | ||
481 | m_angularDeflectionEfficiency = 0; | ||
482 | m_angularDeflectionTimescale = 1000; | ||
483 | m_bankingEfficiency = 0; | ||
484 | m_bankingMix = 1; | ||
485 | m_bankingTimescale = 1000; | ||
486 | m_verticalAttractionEfficiency = 0; | ||
487 | m_verticalAttractionTimescale = 1000; | ||
488 | |||
489 | m_flags = (VehicleFlag)0; | ||
490 | break; | ||
491 | |||
492 | case Vehicle.TYPE_SLED: | ||
493 | m_linearFrictionTimescale = new Vector3(30, 1, 1000); | ||
494 | m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); | ||
495 | m_linearMotorTimescale = 1000; | ||
496 | m_linearMotorDecayTimescale = 120 * m_invtimestep; | ||
497 | m_angularMotorTimescale = 1000; | ||
498 | m_angularMotorDecayTimescale = 120 * m_invtimestep; | ||
499 | m_VhoverHeight = 0; | ||
500 | m_VhoverEfficiency = 1; | ||
501 | m_VhoverTimescale = 10; | ||
502 | m_VehicleBuoyancy = 0; | ||
503 | m_linearDeflectionEfficiency = 1; | ||
504 | m_linearDeflectionTimescale = 1; | ||
505 | m_angularDeflectionEfficiency = 0; | ||
506 | m_angularDeflectionTimescale = 10; | ||
507 | m_verticalAttractionEfficiency = 1; | ||
508 | m_verticalAttractionTimescale = 1000; | ||
509 | m_bankingEfficiency = 0; | ||
510 | m_bankingMix = 1; | ||
511 | m_bankingTimescale = 10; | ||
512 | m_flags &= | ||
513 | ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | | ||
514 | VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); | ||
515 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP | | ||
516 | VehicleFlag.LIMIT_ROLL_ONLY | | ||
517 | VehicleFlag.LIMIT_MOTOR_UP); | ||
518 | break; | ||
519 | |||
520 | case Vehicle.TYPE_CAR: | ||
521 | m_linearFrictionTimescale = new Vector3(100, 2, 1000); | ||
522 | m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); | ||
523 | m_linearMotorTimescale = 1; | ||
524 | m_linearMotorDecayTimescale = 60 * m_invtimestep; | ||
525 | m_angularMotorTimescale = 1; | ||
526 | m_angularMotorDecayTimescale = 0.8f * m_invtimestep; | ||
527 | m_VhoverHeight = 0; | ||
528 | m_VhoverEfficiency = 0; | ||
529 | m_VhoverTimescale = 1000; | ||
530 | m_VehicleBuoyancy = 0; | ||
531 | m_linearDeflectionEfficiency = 1; | ||
532 | m_linearDeflectionTimescale = 2; | ||
533 | m_angularDeflectionEfficiency = 0; | ||
534 | m_angularDeflectionTimescale = 10; | ||
535 | m_verticalAttractionEfficiency = 1f; | ||
536 | m_verticalAttractionTimescale = 10f; | ||
537 | m_bankingEfficiency = -0.2f; | ||
538 | m_bankingMix = 1; | ||
539 | m_bankingTimescale = 1; | ||
540 | m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | | ||
541 | VehicleFlag.HOVER_TERRAIN_ONLY | | ||
542 | VehicleFlag.HOVER_GLOBAL_HEIGHT); | ||
543 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP | | ||
544 | VehicleFlag.LIMIT_ROLL_ONLY | | ||
545 | VehicleFlag.LIMIT_MOTOR_UP | | ||
546 | VehicleFlag.HOVER_UP_ONLY); | ||
547 | break; | ||
548 | case Vehicle.TYPE_BOAT: | ||
549 | m_linearFrictionTimescale = new Vector3(10, 3, 2); | ||
550 | m_angularFrictionTimescale = new Vector3(10, 10, 10); | ||
551 | m_linearMotorTimescale = 5; | ||
552 | m_linearMotorDecayTimescale = 60 * m_invtimestep; | ||
553 | m_angularMotorTimescale = 4; | ||
554 | m_angularMotorDecayTimescale = 4 * m_invtimestep; | ||
555 | m_VhoverHeight = 0; | ||
556 | m_VhoverEfficiency = 0.5f; | ||
557 | m_VhoverTimescale = 2; | ||
558 | m_VehicleBuoyancy = 1; | ||
559 | m_linearDeflectionEfficiency = 0.5f; | ||
560 | m_linearDeflectionTimescale = 3; | ||
561 | m_angularDeflectionEfficiency = 0.5f; | ||
562 | m_angularDeflectionTimescale = 5; | ||
563 | m_verticalAttractionEfficiency = 0.5f; | ||
564 | m_verticalAttractionTimescale = 5f; | ||
565 | m_bankingEfficiency = -0.3f; | ||
566 | m_bankingMix = 0.8f; | ||
567 | m_bankingTimescale = 1; | ||
568 | m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | | ||
569 | VehicleFlag.HOVER_GLOBAL_HEIGHT | | ||
570 | VehicleFlag.HOVER_UP_ONLY); // | | ||
571 | // VehicleFlag.LIMIT_ROLL_ONLY); | ||
572 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP | | ||
573 | VehicleFlag.LIMIT_MOTOR_UP | | ||
574 | VehicleFlag.HOVER_UP_ONLY | // new sl | ||
575 | VehicleFlag.HOVER_WATER_ONLY); | ||
576 | break; | ||
577 | |||
578 | case Vehicle.TYPE_AIRPLANE: | ||
579 | m_linearFrictionTimescale = new Vector3(200, 10, 5); | ||
580 | m_angularFrictionTimescale = new Vector3(20, 20, 20); | ||
581 | m_linearMotorTimescale = 2; | ||
582 | m_linearMotorDecayTimescale = 60 * m_invtimestep; | ||
583 | m_angularMotorTimescale = 4; | ||
584 | m_angularMotorDecayTimescale = 8 * m_invtimestep; | ||
585 | m_VhoverHeight = 0; | ||
586 | m_VhoverEfficiency = 0.5f; | ||
587 | m_VhoverTimescale = 1000; | ||
588 | m_VehicleBuoyancy = 0; | ||
589 | m_linearDeflectionEfficiency = 0.5f; | ||
590 | m_linearDeflectionTimescale = 0.5f; | ||
591 | m_angularDeflectionEfficiency = 1; | ||
592 | m_angularDeflectionTimescale = 2; | ||
593 | m_verticalAttractionEfficiency = 0.9f; | ||
594 | m_verticalAttractionTimescale = 2f; | ||
595 | m_bankingEfficiency = 1; | ||
596 | m_bankingMix = 0.7f; | ||
597 | m_bankingTimescale = 2; | ||
598 | m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | | ||
599 | VehicleFlag.HOVER_TERRAIN_ONLY | | ||
600 | VehicleFlag.HOVER_GLOBAL_HEIGHT | | ||
601 | VehicleFlag.HOVER_UP_ONLY | | ||
602 | VehicleFlag.NO_DEFLECTION_UP | | ||
603 | VehicleFlag.LIMIT_MOTOR_UP); | ||
604 | m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); | ||
605 | break; | ||
606 | |||
607 | case Vehicle.TYPE_BALLOON: | ||
608 | m_linearFrictionTimescale = new Vector3(5, 5, 5); | ||
609 | m_angularFrictionTimescale = new Vector3(10, 10, 10); | ||
610 | m_linearMotorTimescale = 5; | ||
611 | m_linearMotorDecayTimescale = 60 * m_invtimestep; | ||
612 | m_angularMotorTimescale = 6; | ||
613 | m_angularMotorDecayTimescale = 10 * m_invtimestep; | ||
614 | m_VhoverHeight = 5; | ||
615 | m_VhoverEfficiency = 0.8f; | ||
616 | m_VhoverTimescale = 10; | ||
617 | m_VehicleBuoyancy = 1; | ||
618 | m_linearDeflectionEfficiency = 0; | ||
619 | m_linearDeflectionTimescale = 5 * m_invtimestep; | ||
620 | m_angularDeflectionEfficiency = 0; | ||
621 | m_angularDeflectionTimescale = 5; | ||
622 | m_verticalAttractionEfficiency = 1f; | ||
623 | m_verticalAttractionTimescale = 1000f; | ||
624 | m_bankingEfficiency = 0; | ||
625 | m_bankingMix = 0.7f; | ||
626 | m_bankingTimescale = 5; | ||
627 | m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | | ||
628 | VehicleFlag.HOVER_TERRAIN_ONLY | | ||
629 | VehicleFlag.HOVER_UP_ONLY | | ||
630 | VehicleFlag.NO_DEFLECTION_UP | | ||
631 | VehicleFlag.LIMIT_MOTOR_UP | //); | ||
632 | VehicleFlag.LIMIT_ROLL_ONLY | // new sl | ||
633 | VehicleFlag.HOVER_GLOBAL_HEIGHT); // new sl | ||
634 | |||
635 | // m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | | ||
636 | // VehicleFlag.HOVER_GLOBAL_HEIGHT); | ||
637 | break; | ||
638 | |||
639 | } | ||
640 | |||
641 | m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale); | ||
642 | m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; | ||
643 | |||
644 | }//end SetDefaultsForType | ||
645 | |||
646 | internal void Stop() | ||
647 | { | ||
648 | m_lmEfect = 0; | ||
649 | m_lmDecay = 0f; | ||
650 | m_amEfect = 0; | ||
651 | m_amDecay = 0; | ||
652 | m_ffactor = 1f; | ||
653 | } | ||
654 | |||
655 | public static Vector3 Xrot(Quaternion rot) | ||
656 | { | ||
657 | Vector3 vec; | ||
658 | rot.Normalize(); // just in case | ||
659 | vec.X = 2 * (rot.X * rot.X + rot.W * rot.W) - 1; | ||
660 | vec.Y = 2 * (rot.X * rot.Y + rot.Z * rot.W); | ||
661 | vec.Z = 2 * (rot.X * rot.Z - rot.Y * rot.W); | ||
662 | return vec; | ||
663 | } | ||
664 | |||
665 | public static Vector3 Zrot(Quaternion rot) | ||
666 | { | ||
667 | Vector3 vec; | ||
668 | rot.Normalize(); // just in case | ||
669 | vec.X = 2 * (rot.X * rot.Z + rot.Y * rot.W); | ||
670 | vec.Y = 2 * (rot.Y * rot.Z - rot.X * rot.W); | ||
671 | vec.Z = 2 * (rot.Z * rot.Z + rot.W * rot.W) - 1; | ||
672 | |||
673 | return vec; | ||
674 | } | ||
675 | |||
676 | private const float pi = (float)Math.PI; | ||
677 | private const float halfpi = 0.5f * (float)Math.PI; | ||
678 | private const float twopi = 2.0f * pi; | ||
679 | |||
680 | public static Vector3 ubitRot2Euler(Quaternion rot) | ||
681 | { | ||
682 | // returns roll in X | ||
683 | // pitch in Y | ||
684 | // yaw in Z | ||
685 | Vector3 vec; | ||
686 | |||
687 | // assuming rot is normalised | ||
688 | // rot.Normalize(); | ||
689 | |||
690 | float zX = rot.X * rot.Z + rot.Y * rot.W; | ||
691 | |||
692 | if (zX < -0.49999f) | ||
693 | { | ||
694 | vec.X = 0; | ||
695 | vec.Y = -halfpi; | ||
696 | vec.Z = (float)(-2d * Math.Atan(rot.X / rot.W)); | ||
697 | } | ||
698 | else if (zX > 0.49999f) | ||
699 | { | ||
700 | vec.X = 0; | ||
701 | vec.Y = halfpi; | ||
702 | vec.Z = (float)(2d * Math.Atan(rot.X / rot.W)); | ||
703 | } | ||
704 | else | ||
705 | { | ||
706 | vec.Y = (float)Math.Asin(2 * zX); | ||
707 | |||
708 | float sqw = rot.W * rot.W; | ||
709 | |||
710 | float minuszY = rot.X * rot.W - rot.Y * rot.Z; | ||
711 | float zZ = rot.Z * rot.Z + sqw - 0.5f; | ||
712 | |||
713 | vec.X = (float)Math.Atan2(minuszY, zZ); | ||
714 | |||
715 | float yX = rot.Z * rot.W - rot.X * rot.Y; //( have negative ?) | ||
716 | float yY = rot.X * rot.X + sqw - 0.5f; | ||
717 | vec.Z = (float)Math.Atan2(yX, yY); | ||
718 | } | ||
719 | return vec; | ||
720 | } | ||
721 | |||
722 | public static void GetRollPitch(Quaternion rot, out float roll, out float pitch) | ||
723 | { | ||
724 | // assuming rot is normalised | ||
725 | // rot.Normalize(); | ||
726 | |||
727 | float zX = rot.X * rot.Z + rot.Y * rot.W; | ||
728 | |||
729 | if (zX < -0.49999f) | ||
730 | { | ||
731 | roll = 0; | ||
732 | pitch = -halfpi; | ||
733 | } | ||
734 | else if (zX > 0.49999f) | ||
735 | { | ||
736 | roll = 0; | ||
737 | pitch = halfpi; | ||
738 | } | ||
739 | else | ||
740 | { | ||
741 | pitch = (float)Math.Asin(2 * zX); | ||
742 | |||
743 | float minuszY = rot.X * rot.W - rot.Y * rot.Z; | ||
744 | float zZ = rot.Z * rot.Z + rot.W * rot.W - 0.5f; | ||
745 | |||
746 | roll = (float)Math.Atan2(minuszY, zZ); | ||
747 | } | ||
748 | return ; | ||
749 | } | ||
750 | |||
751 | internal void Step() | ||
752 | { | ||
753 | IntPtr Body = rootPrim.Body; | ||
754 | |||
755 | d.Mass dmass; | ||
756 | d.BodyGetMass(Body, out dmass); | ||
757 | |||
758 | d.Quaternion rot = d.BodyGetQuaternion(Body); | ||
759 | Quaternion objrotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object | ||
760 | Quaternion rotq = objrotq; // rotq = rotation of object | ||
761 | rotq *= m_referenceFrame; // rotq is now rotation in vehicle reference frame | ||
762 | Quaternion irotq = Quaternion.Inverse(rotq); | ||
763 | |||
764 | d.Vector3 dvtmp; | ||
765 | Vector3 tmpV; | ||
766 | Vector3 curVel; // velocity in world | ||
767 | Vector3 curAngVel; // angular velocity in world | ||
768 | Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame | ||
769 | Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in vehicle frame | ||
770 | d.Vector3 dtorque = new d.Vector3(); | ||
771 | |||
772 | dvtmp = d.BodyGetLinearVel(Body); | ||
773 | curVel.X = dvtmp.X; | ||
774 | curVel.Y = dvtmp.Y; | ||
775 | curVel.Z = dvtmp.Z; | ||
776 | Vector3 curLocalVel = curVel * irotq; // current velocity in local | ||
777 | |||
778 | dvtmp = d.BodyGetAngularVel(Body); | ||
779 | curAngVel.X = dvtmp.X; | ||
780 | curAngVel.Y = dvtmp.Y; | ||
781 | curAngVel.Z = dvtmp.Z; | ||
782 | Vector3 curLocalAngVel = curAngVel * irotq; // current angular velocity in local | ||
783 | |||
784 | float ldampZ = 0; | ||
785 | |||
786 | // linear motor | ||
787 | if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000) | ||
788 | { | ||
789 | tmpV = m_linearMotorDirection - curLocalVel; // velocity error | ||
790 | tmpV *= m_lmEfect / m_linearMotorTimescale; // error to correct in this timestep | ||
791 | tmpV *= rotq; // to world | ||
792 | |||
793 | if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0) | ||
794 | tmpV.Z = 0; | ||
795 | |||
796 | if (m_linearMotorOffset.X != 0 || m_linearMotorOffset.Y != 0 || m_linearMotorOffset.Z != 0) | ||
797 | { | ||
798 | // have offset, do it now | ||
799 | tmpV *= dmass.mass; | ||
800 | d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z); | ||
801 | } | ||
802 | else | ||
803 | { | ||
804 | force.X += tmpV.X; | ||
805 | force.Y += tmpV.Y; | ||
806 | force.Z += tmpV.Z; | ||
807 | } | ||
808 | |||
809 | m_lmEfect *= m_lmDecay; | ||
810 | // m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared(); | ||
811 | m_ffactor = 0.0f; | ||
812 | } | ||
813 | else | ||
814 | { | ||
815 | m_lmEfect = 0; | ||
816 | m_ffactor = 1f; | ||
817 | } | ||
818 | |||
819 | // hover | ||
820 | if (m_VhoverTimescale < 300 && rootPrim.prim_geom != IntPtr.Zero) | ||
821 | { | ||
822 | // d.Vector3 pos = d.BodyGetPosition(Body); | ||
823 | d.Vector3 pos = d.GeomGetPosition(rootPrim.prim_geom); | ||
824 | pos.Z -= 0.21f; // minor offset that seems to be always there in sl | ||
825 | |||
826 | float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y); | ||
827 | float perr; | ||
828 | |||
829 | // default to global but don't go underground | ||
830 | perr = m_VhoverHeight - pos.Z; | ||
831 | |||
832 | if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0) | ||
833 | { | ||
834 | if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0) | ||
835 | { | ||
836 | perr += _pParentScene.GetWaterLevel(); | ||
837 | } | ||
838 | else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) | ||
839 | { | ||
840 | perr += t; | ||
841 | } | ||
842 | else | ||
843 | { | ||
844 | float w = _pParentScene.GetWaterLevel(); | ||
845 | if (t > w) | ||
846 | perr += t; | ||
847 | else | ||
848 | perr += w; | ||
849 | } | ||
850 | } | ||
851 | else if (t > m_VhoverHeight) | ||
852 | perr = t - pos.Z; ; | ||
853 | |||
854 | if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > -0.1) | ||
855 | { | ||
856 | ldampZ = m_VhoverEfficiency * m_invtimestep; | ||
857 | |||
858 | perr *= (1.0f + ldampZ) / m_VhoverTimescale; | ||
859 | |||
860 | // force.Z += perr - curVel.Z * tmp; | ||
861 | force.Z += perr; | ||
862 | ldampZ *= -curVel.Z; | ||
863 | |||
864 | force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy); | ||
865 | } | ||
866 | else // no buoyancy | ||
867 | force.Z += _pParentScene.gravityz; | ||
868 | } | ||
869 | else | ||
870 | { | ||
871 | // default gravity and Buoyancy | ||
872 | force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy); | ||
873 | } | ||
874 | |||
875 | // linear deflection | ||
876 | if (m_linearDeflectionEfficiency > 0) | ||
877 | { | ||
878 | float len = curVel.Length(); | ||
879 | if (len > 0.01) // if moving | ||
880 | { | ||
881 | Vector3 atAxis; | ||
882 | atAxis = Xrot(rotq); // where are we pointing to | ||
883 | atAxis *= len; // make it same size as world velocity vector | ||
884 | |||
885 | tmpV = -atAxis; // oposite direction | ||
886 | atAxis -= curVel; // error to one direction | ||
887 | len = atAxis.LengthSquared(); | ||
888 | |||
889 | tmpV -= curVel; // error to oposite | ||
890 | float lens = tmpV.LengthSquared(); | ||
891 | |||
892 | if (len > 0.01 || lens > 0.01) // do nothing if close enougth | ||
893 | { | ||
894 | if (len < lens) | ||
895 | tmpV = atAxis; | ||
896 | |||
897 | tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep | ||
898 | force.X += tmpV.X; | ||
899 | force.Y += tmpV.Y; | ||
900 | if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0) | ||
901 | force.Z += tmpV.Z; | ||
902 | } | ||
903 | } | ||
904 | } | ||
905 | |||
906 | // linear friction/damping | ||
907 | if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0) | ||
908 | { | ||
909 | tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X; | ||
910 | tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y; | ||
911 | tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z; | ||
912 | tmpV *= rotq; // to world | ||
913 | |||
914 | if(ldampZ != 0 && Math.Abs(ldampZ) > Math.Abs(tmpV.Z)) | ||
915 | tmpV.Z = ldampZ; | ||
916 | force.X += tmpV.X; | ||
917 | force.Y += tmpV.Y; | ||
918 | force.Z += tmpV.Z; | ||
919 | } | ||
920 | |||
921 | // vertical atractor | ||
922 | if (m_verticalAttractionTimescale < 300) | ||
923 | { | ||
924 | float roll; | ||
925 | float pitch; | ||
926 | |||
927 | |||
928 | |||
929 | float ftmp = m_invtimestep / m_verticalAttractionTimescale / m_verticalAttractionTimescale; | ||
930 | |||
931 | float ftmp2; | ||
932 | ftmp2 = 0.5f * m_verticalAttractionEfficiency * m_invtimestep; | ||
933 | m_amdampX = ftmp2; | ||
934 | |||
935 | m_ampwr = 1.0f - 0.8f * m_verticalAttractionEfficiency; | ||
936 | |||
937 | GetRollPitch(irotq, out roll, out pitch); | ||
938 | |||
939 | if (roll > halfpi) | ||
940 | roll = pi - roll; | ||
941 | else if (roll < -halfpi) | ||
942 | roll = -pi - roll; | ||
943 | |||
944 | float effroll = pitch / halfpi; | ||
945 | effroll *= effroll; | ||
946 | effroll = 1 - effroll; | ||
947 | effroll *= roll; | ||
948 | |||
949 | |||
950 | torque.X += effroll * ftmp; | ||
951 | |||
952 | if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0) | ||
953 | { | ||
954 | float effpitch = roll / halfpi; | ||
955 | effpitch *= effpitch; | ||
956 | effpitch = 1 - effpitch; | ||
957 | effpitch *= pitch; | ||
958 | |||
959 | torque.Y += effpitch * ftmp; | ||
960 | } | ||
961 | |||
962 | if (m_bankingEfficiency != 0 && Math.Abs(effroll) > 0.01) | ||
963 | { | ||
964 | |||
965 | float broll = effroll; | ||
966 | /* | ||
967 | if (broll > halfpi) | ||
968 | broll = pi - broll; | ||
969 | else if (broll < -halfpi) | ||
970 | broll = -pi - broll; | ||
971 | */ | ||
972 | broll *= m_bankingEfficiency; | ||
973 | if (m_bankingMix != 0) | ||
974 | { | ||
975 | float vfact = Math.Abs(curLocalVel.X) / 10.0f; | ||
976 | if (vfact > 1.0f) vfact = 1.0f; | ||
977 | |||
978 | if (curLocalVel.X >= 0) | ||
979 | broll *= (1 + (vfact - 1) * m_bankingMix); | ||
980 | else | ||
981 | broll *= -(1 + (vfact - 1) * m_bankingMix); | ||
982 | } | ||
983 | // make z rot be in world Z not local as seems to be in sl | ||
984 | |||
985 | broll = broll / m_bankingTimescale; | ||
986 | |||
987 | |||
988 | tmpV = Zrot(irotq); | ||
989 | tmpV *= broll; | ||
990 | |||
991 | torque.X += tmpV.X; | ||
992 | torque.Y += tmpV.Y; | ||
993 | torque.Z += tmpV.Z; | ||
994 | |||
995 | m_amdampZ = Math.Abs(m_bankingEfficiency) / m_bankingTimescale; | ||
996 | m_amdampY = m_amdampZ; | ||
997 | |||
998 | } | ||
999 | else | ||
1000 | { | ||
1001 | m_amdampZ = 1 / m_angularFrictionTimescale.Z; | ||
1002 | m_amdampY = m_amdampX; | ||
1003 | } | ||
1004 | } | ||
1005 | else | ||
1006 | { | ||
1007 | m_ampwr = 1.0f; | ||
1008 | m_amdampX = 1 / m_angularFrictionTimescale.X; | ||
1009 | m_amdampY = 1 / m_angularFrictionTimescale.Y; | ||
1010 | m_amdampZ = 1 / m_angularFrictionTimescale.Z; | ||
1011 | } | ||
1012 | |||
1013 | // angular motor | ||
1014 | if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000) | ||
1015 | { | ||
1016 | tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error | ||
1017 | tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep | ||
1018 | torque.X += tmpV.X * m_ampwr; | ||
1019 | torque.Y += tmpV.Y * m_ampwr; | ||
1020 | torque.Z += tmpV.Z; | ||
1021 | |||
1022 | m_amEfect *= m_amDecay; | ||
1023 | } | ||
1024 | else | ||
1025 | m_amEfect = 0; | ||
1026 | |||
1027 | // angular deflection | ||
1028 | if (m_angularDeflectionEfficiency > 0) | ||
1029 | { | ||
1030 | Vector3 dirv; | ||
1031 | |||
1032 | if (curLocalVel.X > 0.01f) | ||
1033 | dirv = curLocalVel; | ||
1034 | else if (curLocalVel.X < -0.01f) | ||
1035 | // use oposite | ||
1036 | dirv = -curLocalVel; | ||
1037 | else | ||
1038 | { | ||
1039 | // make it fall into small positive x case | ||
1040 | dirv.X = 0.01f; | ||
1041 | dirv.Y = curLocalVel.Y; | ||
1042 | dirv.Z = curLocalVel.Z; | ||
1043 | } | ||
1044 | |||
1045 | float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale; | ||
1046 | |||
1047 | if (Math.Abs(dirv.Z) > 0.01) | ||
1048 | { | ||
1049 | torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp; | ||
1050 | } | ||
1051 | |||
1052 | if (Math.Abs(dirv.Y) > 0.01) | ||
1053 | { | ||
1054 | torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp; | ||
1055 | } | ||
1056 | } | ||
1057 | |||
1058 | // angular friction | ||
1059 | if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0) | ||
1060 | { | ||
1061 | torque.X -= curLocalAngVel.X * m_amdampX; | ||
1062 | torque.Y -= curLocalAngVel.Y * m_amdampY; | ||
1063 | torque.Z -= curLocalAngVel.Z * m_amdampZ; | ||
1064 | } | ||
1065 | |||
1066 | |||
1067 | |||
1068 | if (force.X != 0 || force.Y != 0 || force.Z != 0) | ||
1069 | { | ||
1070 | force *= dmass.mass; | ||
1071 | d.BodyAddForce(Body, force.X, force.Y, force.Z); | ||
1072 | } | ||
1073 | |||
1074 | if (torque.X != 0 || torque.Y != 0 || torque.Z != 0) | ||
1075 | { | ||
1076 | torque *= m_referenceFrame; // to object frame | ||
1077 | dtorque.X = torque.X ; | ||
1078 | dtorque.Y = torque.Y; | ||
1079 | dtorque.Z = torque.Z; | ||
1080 | |||
1081 | d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque); | ||
1082 | d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame | ||
1083 | } | ||
1084 | } | ||
1085 | } | ||
1086 | } | ||