clockwerk-opensim – Blame information for rev 1
?pathlinks?
Rev | Author | Line No. | Line |
---|---|---|---|
1 | vero | 1 | /* |
2 | * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces |
||
3 | * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: |
||
4 | * ODEPrim.cs contains methods dealing with Prim editing, Prim |
||
5 | * characteristics and Kinetic motion. |
||
6 | * ODEDynamics.cs contains methods dealing with Prim Physical motion |
||
7 | * (dynamics) and the associated settings. Old Linear and angular |
||
8 | * motors for dynamic motion have been replace with MoveLinear() |
||
9 | * and MoveAngular(); 'Physical' is used only to switch ODE dynamic |
||
10 | * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to |
||
11 | * switch between 'VEHICLE' parameter use and general dynamics |
||
12 | * settings use. |
||
13 | * |
||
14 | * Copyright (c) Contributors, http://opensimulator.org/ |
||
15 | * See CONTRIBUTORS.TXT for a full list of copyright holders. |
||
16 | * |
||
17 | * Redistribution and use in source and binary forms, with or without |
||
18 | * modification, are permitted provided that the following conditions are met: |
||
19 | * * Redistributions of source code must retain the above copyright |
||
20 | * notice, this list of conditions and the following disclaimer. |
||
21 | * * Redistributions in binary form must reproduce the above copyright |
||
22 | * notice, this list of conditions and the following disclaimer in the |
||
23 | * documentation and/or other materials provided with the distribution. |
||
24 | * * Neither the name of the OpenSimulator Project nor the |
||
25 | * names of its contributors may be used to endorse or promote products |
||
26 | * derived from this software without specific prior written permission. |
||
27 | * |
||
28 | * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY |
||
29 | * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED |
||
30 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE |
||
31 | * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY |
||
32 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES |
||
33 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
||
34 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND |
||
35 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT |
||
36 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS |
||
37 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
||
38 | */ |
||
39 | |||
40 | using System; |
||
41 | using System.Collections.Generic; |
||
42 | using System.Reflection; |
||
43 | using System.Runtime.InteropServices; |
||
44 | using log4net; |
||
45 | using OpenMetaverse; |
||
46 | using Ode.NET; |
||
47 | using OpenSim.Framework; |
||
48 | using OpenSim.Region.Physics.Manager; |
||
49 | |||
50 | namespace OpenSim.Region.Physics.OdePlugin |
||
51 | { |
||
52 | public class ODEDynamics |
||
53 | { |
||
54 | public Vehicle Type |
||
55 | { |
||
56 | get { return m_type; } |
||
57 | } |
||
58 | |||
59 | public IntPtr Body |
||
60 | { |
||
61 | get { return m_body; } |
||
62 | } |
||
63 | |||
64 | private int frcount = 0; // Used to limit dynamics debug output to |
||
65 | // every 100th frame |
||
66 | |||
67 | // private OdeScene m_parentScene = null; |
||
68 | private IntPtr m_body = IntPtr.Zero; |
||
69 | private IntPtr m_jointGroup = IntPtr.Zero; |
||
70 | private IntPtr m_aMotor = IntPtr.Zero; |
||
71 | |||
72 | |||
73 | // Vehicle properties |
||
74 | private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind |
||
75 | // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier |
||
76 | private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings: |
||
77 | // HOVER_TERRAIN_ONLY |
||
78 | // HOVER_GLOBAL_HEIGHT |
||
79 | // NO_DEFLECTION_UP |
||
80 | // HOVER_WATER_ONLY |
||
81 | // HOVER_UP_ONLY |
||
82 | // LIMIT_MOTOR_UP |
||
83 | // LIMIT_ROLL_ONLY |
||
84 | |||
85 | // Linear properties |
||
86 | private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time |
||
87 | private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL |
||
88 | private Vector3 m_dir = Vector3.Zero; // velocity applied to body |
||
89 | private Vector3 m_linearFrictionTimescale = Vector3.Zero; |
||
90 | private float m_linearMotorDecayTimescale = 0; |
||
91 | private float m_linearMotorTimescale = 0; |
||
92 | private Vector3 m_lastLinearVelocityVector = Vector3.Zero; |
||
93 | // private bool m_LinearMotorSetLastFrame = false; |
||
94 | // private Vector3 m_linearMotorOffset = Vector3.Zero; |
||
95 | |||
96 | //Angular properties |
||
97 | private Vector3 m_angularMotorDirection = Vector3.Zero; |
||
98 | private Vector3 m_angularMotorDirectionLASTSET = Vector3.Zero; |
||
99 | private Vector3 m_angularFrictionTimescale = Vector3.Zero; |
||
100 | private float m_angularMotorDecayTimescale = 0; |
||
101 | private float m_angularMotorTimescale = 0; |
||
102 | private Vector3 m_lastAngularVelocityVector = Vector3.Zero; |
||
103 | |||
104 | //Deflection properties |
||
105 | // private float m_angularDeflectionEfficiency = 0; |
||
106 | // private float m_angularDeflectionTimescale = 0; |
||
107 | // private float m_linearDeflectionEfficiency = 0; |
||
108 | // private float m_linearDeflectionTimescale = 0; |
||
109 | |||
110 | //Banking properties |
||
111 | // private float m_bankingEfficiency = 0; |
||
112 | // private float m_bankingMix = 0; |
||
113 | // private float m_bankingTimescale = 0; |
||
114 | |||
115 | //Hover and Buoyancy properties |
||
116 | private float m_VhoverHeight = 0f; |
||
117 | private float m_VhoverEfficiency = 0f; |
||
118 | private float m_VhoverTimescale = 0f; |
||
119 | private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height |
||
120 | private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle. |
||
121 | // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) |
||
122 | // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. |
||
123 | // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. |
||
124 | |||
125 | //Attractor properties |
||
126 | private float m_verticalAttractionEfficiency = 0; |
||
127 | private float m_verticalAttractionTimescale = 0; |
||
128 | |||
129 | |||
130 | |||
131 | |||
132 | |||
133 | internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) |
||
134 | { |
||
135 | switch (pParam) |
||
136 | { |
||
137 | case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY: |
||
138 | if (pValue < 0.01f) pValue = 0.01f; |
||
139 | // m_angularDeflectionEfficiency = pValue; |
||
140 | break; |
||
141 | case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: |
||
142 | if (pValue < 0.01f) pValue = 0.01f; |
||
143 | // m_angularDeflectionTimescale = pValue; |
||
144 | break; |
||
145 | case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: |
||
146 | if (pValue < 0.01f) pValue = 0.01f; |
||
147 | m_angularMotorDecayTimescale = pValue; |
||
148 | break; |
||
149 | case Vehicle.ANGULAR_MOTOR_TIMESCALE: |
||
150 | if (pValue < 0.01f) pValue = 0.01f; |
||
151 | m_angularMotorTimescale = pValue; |
||
152 | break; |
||
153 | case Vehicle.BANKING_EFFICIENCY: |
||
154 | if (pValue < 0.01f) pValue = 0.01f; |
||
155 | // m_bankingEfficiency = pValue; |
||
156 | break; |
||
157 | case Vehicle.BANKING_MIX: |
||
158 | if (pValue < 0.01f) pValue = 0.01f; |
||
159 | // m_bankingMix = pValue; |
||
160 | break; |
||
161 | case Vehicle.BANKING_TIMESCALE: |
||
162 | if (pValue < 0.01f) pValue = 0.01f; |
||
163 | // m_bankingTimescale = pValue; |
||
164 | break; |
||
165 | case Vehicle.BUOYANCY: |
||
166 | if (pValue < -1f) pValue = -1f; |
||
167 | if (pValue > 1f) pValue = 1f; |
||
168 | m_VehicleBuoyancy = pValue; |
||
169 | break; |
||
170 | case Vehicle.HOVER_EFFICIENCY: |
||
171 | if (pValue < 0f) pValue = 0f; |
||
172 | if (pValue > 1f) pValue = 1f; |
||
173 | m_VhoverEfficiency = pValue; |
||
174 | break; |
||
175 | case Vehicle.HOVER_HEIGHT: |
||
176 | m_VhoverHeight = pValue; |
||
177 | break; |
||
178 | case Vehicle.HOVER_TIMESCALE: |
||
179 | if (pValue < 0.01f) pValue = 0.01f; |
||
180 | m_VhoverTimescale = pValue; |
||
181 | break; |
||
182 | case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: |
||
183 | if (pValue < 0.01f) pValue = 0.01f; |
||
184 | // m_linearDeflectionEfficiency = pValue; |
||
185 | break; |
||
186 | case Vehicle.LINEAR_DEFLECTION_TIMESCALE: |
||
187 | if (pValue < 0.01f) pValue = 0.01f; |
||
188 | // m_linearDeflectionTimescale = pValue; |
||
189 | break; |
||
190 | case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: |
||
191 | if (pValue < 0.01f) pValue = 0.01f; |
||
192 | m_linearMotorDecayTimescale = pValue; |
||
193 | break; |
||
194 | case Vehicle.LINEAR_MOTOR_TIMESCALE: |
||
195 | if (pValue < 0.01f) pValue = 0.01f; |
||
196 | m_linearMotorTimescale = pValue; |
||
197 | break; |
||
198 | case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: |
||
199 | if (pValue < 0.0f) pValue = 0.0f; |
||
200 | if (pValue > 1.0f) pValue = 1.0f; |
||
201 | m_verticalAttractionEfficiency = pValue; |
||
202 | break; |
||
203 | case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: |
||
204 | if (pValue < 0.01f) pValue = 0.01f; |
||
205 | m_verticalAttractionTimescale = pValue; |
||
206 | break; |
||
207 | |||
208 | // These are vector properties but the engine lets you use a single float value to |
||
209 | // set all of the components to the same value |
||
210 | case Vehicle.ANGULAR_FRICTION_TIMESCALE: |
||
211 | m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); |
||
212 | break; |
||
213 | case Vehicle.ANGULAR_MOTOR_DIRECTION: |
||
214 | m_angularMotorDirection = new Vector3(pValue, pValue, pValue); |
||
215 | m_angularMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue); |
||
216 | break; |
||
217 | case Vehicle.LINEAR_FRICTION_TIMESCALE: |
||
218 | m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); |
||
219 | break; |
||
220 | case Vehicle.LINEAR_MOTOR_DIRECTION: |
||
221 | m_linearMotorDirection = new Vector3(pValue, pValue, pValue); |
||
222 | m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue); |
||
223 | break; |
||
224 | case Vehicle.LINEAR_MOTOR_OFFSET: |
||
225 | // m_linearMotorOffset = new Vector3(pValue, pValue, pValue); |
||
226 | break; |
||
227 | |||
228 | } |
||
229 | |||
230 | }//end ProcessFloatVehicleParam |
||
231 | |||
232 | internal void ProcessVectorVehicleParam(Vehicle pParam, PhysicsVector pValue) |
||
233 | { |
||
234 | switch (pParam) |
||
235 | { |
||
236 | case Vehicle.ANGULAR_FRICTION_TIMESCALE: |
||
237 | m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); |
||
238 | break; |
||
239 | case Vehicle.ANGULAR_MOTOR_DIRECTION: |
||
240 | m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); |
||
241 | m_angularMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); |
||
242 | break; |
||
243 | case Vehicle.LINEAR_FRICTION_TIMESCALE: |
||
244 | m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); |
||
245 | break; |
||
246 | case Vehicle.LINEAR_MOTOR_DIRECTION: |
||
247 | m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); |
||
248 | m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); |
||
249 | break; |
||
250 | case Vehicle.LINEAR_MOTOR_OFFSET: |
||
251 | // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); |
||
252 | break; |
||
253 | } |
||
254 | |||
255 | }//end ProcessVectorVehicleParam |
||
256 | |||
257 | internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) |
||
258 | { |
||
259 | switch (pParam) |
||
260 | { |
||
261 | case Vehicle.REFERENCE_FRAME: |
||
262 | // m_referenceFrame = pValue; |
||
263 | break; |
||
264 | } |
||
265 | |||
266 | }//end ProcessRotationVehicleParam |
||
267 | |||
268 | internal void ProcessTypeChange(Vehicle pType) |
||
269 | { |
||
270 | Console.WriteLine("ProcessTypeChange to " + pType); |
||
271 | |||
272 | // Set Defaults For Type |
||
273 | m_type = pType; |
||
274 | switch (pType) |
||
275 | { |
||
276 | case Vehicle.TYPE_SLED: |
||
277 | m_linearFrictionTimescale = new Vector3(30, 1, 1000); |
||
278 | m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); |
||
279 | m_linearMotorDirection = Vector3.Zero; |
||
280 | m_linearMotorTimescale = 1000; |
||
281 | m_linearMotorDecayTimescale = 120; |
||
282 | m_angularMotorDirection = Vector3.Zero; |
||
283 | m_angularMotorTimescale = 1000; |
||
284 | m_angularMotorDecayTimescale = 120; |
||
285 | m_VhoverHeight = 0; |
||
286 | m_VhoverEfficiency = 1; |
||
287 | m_VhoverTimescale = 10; |
||
288 | m_VehicleBuoyancy = 0; |
||
289 | // m_linearDeflectionEfficiency = 1; |
||
290 | // m_linearDeflectionTimescale = 1; |
||
291 | // m_angularDeflectionEfficiency = 1; |
||
292 | // m_angularDeflectionTimescale = 1000; |
||
293 | // m_bankingEfficiency = 0; |
||
294 | // m_bankingMix = 1; |
||
295 | // m_bankingTimescale = 10; |
||
296 | // m_referenceFrame = Quaternion.Identity; |
||
297 | m_flags &= |
||
298 | ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | |
||
299 | VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); |
||
300 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP); |
||
301 | break; |
||
302 | case Vehicle.TYPE_CAR: |
||
303 | m_linearFrictionTimescale = new Vector3(100, 2, 1000); |
||
304 | m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); |
||
305 | m_linearMotorDirection = Vector3.Zero; |
||
306 | m_linearMotorTimescale = 1; |
||
307 | m_linearMotorDecayTimescale = 60; |
||
308 | m_angularMotorDirection = Vector3.Zero; |
||
309 | m_angularMotorTimescale = 1; |
||
310 | m_angularMotorDecayTimescale = 0.8f; |
||
311 | m_VhoverHeight = 0; |
||
312 | m_VhoverEfficiency = 0; |
||
313 | m_VhoverTimescale = 1000; |
||
314 | m_VehicleBuoyancy = 0; |
||
315 | // // m_linearDeflectionEfficiency = 1; |
||
316 | // // m_linearDeflectionTimescale = 2; |
||
317 | // // m_angularDeflectionEfficiency = 0; |
||
318 | // m_angularDeflectionTimescale = 10; |
||
319 | m_verticalAttractionEfficiency = 1; |
||
320 | m_verticalAttractionTimescale = 10; |
||
321 | // m_bankingEfficiency = -0.2f; |
||
322 | // m_bankingMix = 1; |
||
323 | // m_bankingTimescale = 1; |
||
324 | // m_referenceFrame = Quaternion.Identity; |
||
325 | m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); |
||
326 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY | |
||
327 | VehicleFlag.LIMIT_MOTOR_UP); |
||
328 | break; |
||
329 | case Vehicle.TYPE_BOAT: |
||
330 | m_linearFrictionTimescale = new Vector3(10, 3, 2); |
||
331 | m_angularFrictionTimescale = new Vector3(10,10,10); |
||
332 | m_linearMotorDirection = Vector3.Zero; |
||
333 | m_linearMotorTimescale = 5; |
||
334 | m_linearMotorDecayTimescale = 60; |
||
335 | m_angularMotorDirection = Vector3.Zero; |
||
336 | m_angularMotorTimescale = 4; |
||
337 | m_angularMotorDecayTimescale = 4; |
||
338 | m_VhoverHeight = 0; |
||
339 | m_VhoverEfficiency = 0.5f; |
||
340 | m_VhoverTimescale = 2; |
||
341 | m_VehicleBuoyancy = 1; |
||
342 | // m_linearDeflectionEfficiency = 0.5f; |
||
343 | // m_linearDeflectionTimescale = 3; |
||
344 | // m_angularDeflectionEfficiency = 0.5f; |
||
345 | // m_angularDeflectionTimescale = 5; |
||
346 | m_verticalAttractionEfficiency = 0.5f; |
||
347 | m_verticalAttractionTimescale = 5; |
||
348 | // m_bankingEfficiency = -0.3f; |
||
349 | // m_bankingMix = 0.8f; |
||
350 | // m_bankingTimescale = 1; |
||
351 | // m_referenceFrame = Quaternion.Identity; |
||
352 | m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY | |
||
353 | VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); |
||
354 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | |
||
355 | VehicleFlag.LIMIT_MOTOR_UP); |
||
356 | break; |
||
357 | case Vehicle.TYPE_AIRPLANE: |
||
358 | m_linearFrictionTimescale = new Vector3(200, 10, 5); |
||
359 | m_angularFrictionTimescale = new Vector3(20, 20, 20); |
||
360 | m_linearMotorDirection = Vector3.Zero; |
||
361 | m_linearMotorTimescale = 2; |
||
362 | m_linearMotorDecayTimescale = 60; |
||
363 | m_angularMotorDirection = Vector3.Zero; |
||
364 | m_angularMotorTimescale = 4; |
||
365 | m_angularMotorDecayTimescale = 4; |
||
366 | m_VhoverHeight = 0; |
||
367 | m_VhoverEfficiency = 0.5f; |
||
368 | m_VhoverTimescale = 1000; |
||
369 | m_VehicleBuoyancy = 0; |
||
370 | // m_linearDeflectionEfficiency = 0.5f; |
||
371 | // m_linearDeflectionTimescale = 3; |
||
372 | // m_angularDeflectionEfficiency = 1; |
||
373 | // m_angularDeflectionTimescale = 2; |
||
374 | m_verticalAttractionEfficiency = 0.9f; |
||
375 | m_verticalAttractionTimescale = 2; |
||
376 | // m_bankingEfficiency = 1; |
||
377 | // m_bankingMix = 0.7f; |
||
378 | // m_bankingTimescale = 2; |
||
379 | // m_referenceFrame = Quaternion.Identity; |
||
380 | m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | |
||
381 | VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP); |
||
382 | m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); |
||
383 | break; |
||
384 | case Vehicle.TYPE_BALLOON: |
||
385 | m_linearFrictionTimescale = new Vector3(5, 5, 5); |
||
386 | m_angularFrictionTimescale = new Vector3(10, 10, 10); |
||
387 | m_linearMotorDirection = Vector3.Zero; |
||
388 | m_linearMotorTimescale = 5; |
||
389 | m_linearMotorDecayTimescale = 60; |
||
390 | m_angularMotorDirection = Vector3.Zero; |
||
391 | m_angularMotorTimescale = 6; |
||
392 | m_angularMotorDecayTimescale = 10; |
||
393 | m_VhoverHeight = 5; |
||
394 | m_VhoverEfficiency = 0.8f; |
||
395 | m_VhoverTimescale = 10; |
||
396 | m_VehicleBuoyancy = 1; |
||
397 | // m_linearDeflectionEfficiency = 0; |
||
398 | // m_linearDeflectionTimescale = 5; |
||
399 | // m_angularDeflectionEfficiency = 0; |
||
400 | // m_angularDeflectionTimescale = 5; |
||
401 | m_verticalAttractionEfficiency = 1; |
||
402 | m_verticalAttractionTimescale = 1000; |
||
403 | // m_bankingEfficiency = 0; |
||
404 | // m_bankingMix = 0.7f; |
||
405 | // m_bankingTimescale = 5; |
||
406 | // m_referenceFrame = Quaternion.Identity; |
||
407 | m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | |
||
408 | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP); |
||
409 | m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); |
||
410 | break; |
||
411 | |||
412 | } |
||
413 | }//end SetDefaultsForType |
||
414 | |||
415 | internal void Enable(IntPtr pBody, OdeScene pParentScene) |
||
416 | { |
||
417 | //Console.WriteLine("Enable m_type=" + m_type + " m_VehicleBuoyancy=" + m_VehicleBuoyancy); |
||
418 | if (m_type == Vehicle.TYPE_NONE) |
||
419 | return; |
||
420 | |||
421 | m_body = pBody; |
||
422 | //KF: This used to set up the linear and angular joints |
||
423 | } |
||
424 | |||
425 | internal void Step(float pTimestep, OdeScene pParentScene) |
||
426 | { |
||
427 | if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE) |
||
428 | return; |
||
429 | frcount++; // used to limit debug comment output |
||
430 | if (frcount > 100) |
||
431 | frcount = 0; |
||
432 | |||
433 | MoveLinear(pTimestep, pParentScene); |
||
434 | MoveAngular(pTimestep); |
||
435 | }// end Step |
||
436 | |||
437 | private void MoveLinear(float pTimestep, OdeScene _pParentScene) |
||
438 | { |
||
439 | if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant |
||
440 | { |
||
441 | if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); |
||
442 | |||
443 | // add drive to body |
||
444 | Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep); |
||
445 | m_lastLinearVelocityVector += (addAmount*10); // lastLinearVelocityVector is the current body velocity vector? |
||
446 | |||
447 | // This will work temporarily, but we really need to compare speed on an axis |
||
448 | // KF: Limit body velocity to applied velocity? |
||
449 | if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X)) |
||
450 | m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X; |
||
451 | if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y)) |
||
452 | m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y; |
||
453 | if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z)) |
||
454 | m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z; |
||
455 | |||
456 | // decay applied velocity |
||
457 | Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep))); |
||
458 | //Console.WriteLine("decay: " + decayfraction); |
||
459 | m_linearMotorDirection -= m_linearMotorDirection * decayfraction; |
||
460 | //Console.WriteLine("actual: " + m_linearMotorDirection); |
||
461 | } |
||
462 | else |
||
463 | { // requested is not significant |
||
464 | // if what remains of applied is small, zero it. |
||
465 | if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f)) |
||
466 | m_lastLinearVelocityVector = Vector3.Zero; |
||
467 | } |
||
468 | |||
469 | |||
470 | // convert requested object velocity to world-referenced vector |
||
471 | m_dir = m_lastLinearVelocityVector; |
||
472 | d.Quaternion rot = d.BodyGetQuaternion(Body); |
||
473 | Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object |
||
474 | m_dir *= rotq; // apply obj rotation to velocity vector |
||
475 | |||
476 | // add Gravity andBuoyancy |
||
477 | // KF: So far I have found no good method to combine a script-requested |
||
478 | // .Z velocity and gravity. Therefore only 0g will used script-requested |
||
479 | // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only. |
||
480 | Vector3 grav = Vector3.Zero; |
||
481 | if(m_VehicleBuoyancy < 1.0f) |
||
482 | { |
||
483 | // There is some gravity, make a gravity force vector |
||
484 | // that is applied after object velocity. |
||
485 | d.Mass objMass; |
||
486 | d.BodyGetMass(Body, out objMass); |
||
487 | // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; |
||
488 | grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); |
||
489 | // Preserve the current Z velocity |
||
490 | d.Vector3 vel_now = d.BodyGetLinearVel(Body); |
||
491 | m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity |
||
492 | } // else its 1.0, no gravity. |
||
493 | |||
494 | // Check if hovering |
||
495 | if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) |
||
496 | { |
||
497 | // We should hover, get the target height |
||
498 | d.Vector3 pos = d.BodyGetPosition(Body); |
||
499 | if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY) |
||
500 | { |
||
501 | m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight; |
||
502 | } |
||
503 | else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY) |
||
504 | { |
||
505 | m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight; |
||
506 | } |
||
507 | else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT) |
||
508 | { |
||
509 | m_VhoverTargetHeight = m_VhoverHeight; |
||
510 | } |
||
511 | |||
512 | if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY) |
||
513 | { |
||
514 | // If body is aready heigher, use its height as target height |
||
515 | if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z; |
||
516 | } |
||
517 | |||
518 | // m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped |
||
519 | // m_VhoverTimescale = 0f; // time to acheive height |
||
520 | // pTimestep is time since last frame,in secs |
||
521 | float herr0 = pos.Z - m_VhoverTargetHeight; |
||
522 | //if(frcount == 0) Console.WriteLine("herr0=" + herr0); |
||
523 | // Replace Vertical speed with correction figure if significant |
||
524 | if(Math.Abs(herr0) > 0.01f ) |
||
525 | { |
||
526 | d.Mass objMass; |
||
527 | d.BodyGetMass(Body, out objMass); |
||
528 | m_dir.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale); |
||
529 | // m_VhoverEfficiency is not yet implemented |
||
530 | } |
||
531 | else |
||
532 | { |
||
533 | m_dir.Z = 0f; |
||
534 | } |
||
535 | } |
||
536 | |||
537 | // Apply velocity |
||
538 | d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z); |
||
539 | //if(frcount == 0) Console.WriteLine("Move " + Body + ":"+ m_dir.X + " " + m_dir.Y + " " + m_dir.Z); |
||
540 | // apply gravity force |
||
541 | d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); |
||
542 | //if(frcount == 0) Console.WriteLine("Force " + Body + ":" + grav.X + " " + grav.Y + " " + grav.Z); |
||
543 | |||
544 | |||
545 | // apply friction |
||
546 | Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep); |
||
547 | m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount; |
||
548 | } // end MoveLinear() |
||
549 | |||
550 | private void MoveAngular(float pTimestep) |
||
551 | { |
||
552 | |||
553 | // m_angularMotorDirection is the latest value from the script, and is decayed here |
||
554 | // m_angularMotorDirectionLASTSET is the latest value from the script |
||
555 | // m_lastAngularVelocityVector is what is being applied to the Body, varied up and down here |
||
556 | |||
557 | if (!m_angularMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) |
||
558 | { |
||
559 | if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); |
||
560 | // ramp up to new value |
||
561 | Vector3 addAmount = m_angularMotorDirection / (m_angularMotorTimescale / pTimestep); |
||
562 | m_lastAngularVelocityVector += (addAmount * 10f); |
||
563 | //if(frcount == 0) Console.WriteLine("add: " + addAmount); |
||
564 | |||
565 | // limit applied value to what was set by script |
||
566 | // This will work temporarily, but we really need to compare speed on an axis |
||
567 | if (Math.Abs(m_lastAngularVelocityVector.X) > Math.Abs(m_angularMotorDirectionLASTSET.X)) |
||
568 | m_lastAngularVelocityVector.X = m_angularMotorDirectionLASTSET.X; |
||
569 | if (Math.Abs(m_lastAngularVelocityVector.Y) > Math.Abs(m_angularMotorDirectionLASTSET.Y)) |
||
570 | m_lastAngularVelocityVector.Y = m_angularMotorDirectionLASTSET.Y; |
||
571 | if (Math.Abs(m_lastAngularVelocityVector.Z) > Math.Abs(m_angularMotorDirectionLASTSET.Z)) |
||
572 | m_lastAngularVelocityVector.Z = m_angularMotorDirectionLASTSET.Z; |
||
573 | |||
574 | // decay the requested value |
||
575 | Vector3 decayfraction = ((Vector3.One / (m_angularMotorDecayTimescale / pTimestep))); |
||
576 | //Console.WriteLine("decay: " + decayfraction); |
||
577 | m_angularMotorDirection -= m_angularMotorDirection * decayfraction; |
||
578 | //Console.WriteLine("actual: " + m_linearMotorDirection); |
||
579 | } |
||
580 | // KF: m_lastAngularVelocityVector is rotational speed in rad/sec ? |
||
581 | |||
582 | // Vertical attractor section |
||
583 | |||
584 | // d.Mass objMass; |
||
585 | // d.BodyGetMass(Body, out objMass); |
||
586 | // float servo = 100f * objMass.mass * m_verticalAttractionEfficiency / (m_verticalAttractionTimescale * pTimestep); |
||
587 | float servo = 0.1f * m_verticalAttractionEfficiency / (m_verticalAttractionTimescale * pTimestep); |
||
588 | // get present body rotation |
||
589 | d.Quaternion rot = d.BodyGetQuaternion(Body); |
||
590 | Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); |
||
591 | // make a vector pointing up |
||
592 | Vector3 verterr = Vector3.Zero; |
||
593 | verterr.Z = 1.0f; |
||
594 | // rotate it to Body Angle |
||
595 | verterr = verterr * rotq; |
||
596 | // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1. |
||
597 | // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go |
||
598 | // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. |
||
599 | if (verterr.Z < 0.0f) |
||
600 | { |
||
601 | verterr.X = 2.0f - verterr.X; |
||
602 | verterr.Y = 2.0f - verterr.Y; |
||
603 | } |
||
604 | // Error is 0 (no error) to +/- 2 (max error) |
||
605 | // scale it by servo |
||
606 | verterr = verterr * servo; |
||
607 | |||
608 | // rotate to object frame |
||
609 | // verterr = verterr * rotq; |
||
610 | |||
611 | // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so |
||
612 | // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. |
||
613 | m_lastAngularVelocityVector.X += verterr.Y; |
||
614 | m_lastAngularVelocityVector.Y -= verterr.X; |
||
615 | /* |
||
616 | if(frcount == 0) |
||
617 | { |
||
618 | // Console.WriteLine("AngleMotor " + m_lastAngularVelocityVector); |
||
619 | Console.WriteLine(String.Format("VA Body:{0} servo:{1} err:<{2},{3},{4}> VAE:{5}", |
||
620 | Body, servo, verterr.X, verterr.Y, verterr.Z, m_verticalAttractionEfficiency)); |
||
621 | } |
||
622 | */ |
||
623 | d.BodySetAngularVel (Body, m_lastAngularVelocityVector.X, m_lastAngularVelocityVector.Y, m_lastAngularVelocityVector.Z); |
||
624 | // apply friction |
||
625 | Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep); |
||
626 | m_lastAngularVelocityVector -= m_lastAngularVelocityVector * decayamount; |
||
627 | |||
628 | } //end MoveAngular |
||
629 | } |
||
630 | } |