diff options
author | Robert Adams | 2015-09-08 06:15:46 -0700 |
---|---|---|
committer | Robert Adams | 2015-09-08 06:15:46 -0700 |
commit | 4dd17c4117ea413fb0c4418511956cb3abfe258c (patch) | |
tree | 6f31a583610f8bf074d1f0b4e7ab6ccef37dec9a /OpenSim/Region/PhysicsModules/UbitOdePlugin/ODEDynamics.cs | |
parent | Merge of ubitworkvarnew with opensim/master as of 20150905. (diff) | |
download | opensim-SC_OLD-4dd17c4117ea413fb0c4418511956cb3abfe258c.zip opensim-SC_OLD-4dd17c4117ea413fb0c4418511956cb3abfe258c.tar.gz opensim-SC_OLD-4dd17c4117ea413fb0c4418511956cb3abfe258c.tar.bz2 opensim-SC_OLD-4dd17c4117ea413fb0c4418511956cb3abfe258c.tar.xz |
More 'everything is a module' merging.
Have most of UbitOde converted.
There are compile errors in OpenSimBase as the new modules stuff is not all there.
Removed ChOdePlugin as it's connection to OdePlugin was tangled.
Diffstat (limited to 'OpenSim/Region/PhysicsModules/UbitOdePlugin/ODEDynamics.cs')
-rw-r--r-- | OpenSim/Region/PhysicsModules/UbitOdePlugin/ODEDynamics.cs | 1096 |
1 files changed, 1096 insertions, 0 deletions
diff --git a/OpenSim/Region/PhysicsModules/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/PhysicsModules/UbitOdePlugin/ODEDynamics.cs new file mode 100644 index 0000000..36bb1e9 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/UbitOdePlugin/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.PhysicsModules.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 | 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 ubitRot2Euler(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 | } | ||