aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/OdePlugin/ODEVehicleSettings.cs
diff options
context:
space:
mode:
authoropensim2009-09-30 18:51:02 +0200
committeropensim2009-09-30 18:51:02 +0200
commit827b0fb1993c6f9b1289931a1ac38ff2b810952c (patch)
tree3c5e7b7fe5b21fdf159d64f1264a9d41ceac7b69 /OpenSim/Region/Physics/OdePlugin/ODEVehicleSettings.cs
parentFinish the (untested) authentication connector (diff)
downloadopensim-SC-827b0fb1993c6f9b1289931a1ac38ff2b810952c.zip
opensim-SC-827b0fb1993c6f9b1289931a1ac38ff2b810952c.tar.gz
opensim-SC-827b0fb1993c6f9b1289931a1ac38ff2b810952c.tar.bz2
opensim-SC-827b0fb1993c6f9b1289931a1ac38ff2b810952c.tar.xz
Commit initial version of KittoFlora's vehicle changes
Diffstat (limited to 'OpenSim/Region/Physics/OdePlugin/ODEVehicleSettings.cs')
-rw-r--r--OpenSim/Region/Physics/OdePlugin/ODEVehicleSettings.cs626
1 files changed, 0 insertions, 626 deletions
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEVehicleSettings.cs b/OpenSim/Region/Physics/OdePlugin/ODEVehicleSettings.cs
deleted file mode 100644
index a547c3e..0000000
--- a/OpenSim/Region/Physics/OdePlugin/ODEVehicleSettings.cs
+++ /dev/null
@@ -1,626 +0,0 @@
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
28using System;
29using System.Collections.Generic;
30using System.Reflection;
31using System.Runtime.InteropServices;
32using log4net;
33using OpenMetaverse;
34using Ode.NET;
35using OpenSim.Framework;
36using OpenSim.Region.Physics.Manager;
37
38namespace OpenSim.Region.Physics.OdePlugin
39{
40 public class ODEVehicleSettings
41 {
42 public Vehicle Type
43 {
44 get { return m_type; }
45 }
46
47 public IntPtr Body
48 {
49 get { return m_body; }
50 }
51
52 private int frcount = 0;
53 // private float frmod = 3.0f;
54
55 private Vehicle m_type = Vehicle.TYPE_NONE;
56 // private OdeScene m_parentScene = null;
57 private IntPtr m_body = IntPtr.Zero;
58 private IntPtr m_jointGroup = IntPtr.Zero;
59 private IntPtr m_aMotor = IntPtr.Zero;
60 private IntPtr m_lMotor1 = IntPtr.Zero;
61 // private IntPtr m_lMotor2 = IntPtr.Zero;
62 // private IntPtr m_lMotor3 = IntPtr.Zero;
63
64 // Vehicle properties
65 // private Quaternion m_referenceFrame = Quaternion.Identity;
66 private Vector3 m_angularFrictionTimescale = Vector3.Zero;
67 private Vector3 m_angularMotorDirection = Vector3.Zero;
68 private Vector3 m_angularMotorDirectionLASTSET = Vector3.Zero;
69 private Vector3 m_linearFrictionTimescale = Vector3.Zero;
70 private Vector3 m_linearMotorDirection = Vector3.Zero;
71 private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero;
72 // private Vector3 m_linearMotorOffset = Vector3.Zero;
73 // private float m_angularDeflectionEfficiency = 0;
74 // private float m_angularDeflectionTimescale = 0;
75 private float m_angularMotorDecayTimescale = 0;
76 private float m_angularMotorTimescale = 0;
77 // private float m_bankingEfficiency = 0;
78 // private float m_bankingMix = 0;
79 // private float m_bankingTimescale = 0;
80 // private float m_buoyancy = 0;
81 // private float m_hoverHeight = 0;
82 // private float m_hoverEfficiency = 0;
83 // private float m_hoverTimescale = 0;
84 // private float m_linearDeflectionEfficiency = 0;
85 // private float m_linearDeflectionTimescale = 0;
86 private float m_linearMotorDecayTimescale = 0;
87 private float m_linearMotorTimescale = 0;
88 private float m_verticalAttractionEfficiency = 0;
89 private float m_verticalAttractionTimescale = 0;
90 private Vector3 m_lastLinearVelocityVector = Vector3.Zero;
91 private Vector3 m_lastAngularVelocityVector = Vector3.Zero;
92 private VehicleFlag m_flags = (VehicleFlag) 0;
93
94 // private bool m_LinearMotorSetLastFrame = false;
95
96
97
98
99 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
100 {
101 switch (pParam)
102 {
103 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
104 if (pValue < 0.01f) pValue = 0.01f;
105 // m_angularDeflectionEfficiency = pValue;
106 break;
107 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
108 if (pValue < 0.01f) pValue = 0.01f;
109 // m_angularDeflectionTimescale = pValue;
110 break;
111 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
112 if (pValue < 0.01f) pValue = 0.01f;
113 m_angularMotorDecayTimescale = pValue;
114 break;
115 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
116 if (pValue < 0.01f) pValue = 0.01f;
117 m_angularMotorTimescale = pValue;
118 break;
119 case Vehicle.BANKING_EFFICIENCY:
120 if (pValue < 0.01f) pValue = 0.01f;
121 // m_bankingEfficiency = pValue;
122 break;
123 case Vehicle.BANKING_MIX:
124 if (pValue < 0.01f) pValue = 0.01f;
125 // m_bankingMix = pValue;
126 break;
127 case Vehicle.BANKING_TIMESCALE:
128 if (pValue < 0.01f) pValue = 0.01f;
129 // m_bankingTimescale = pValue;
130 break;
131 case Vehicle.BUOYANCY:
132 // m_buoyancy = pValue;
133 break;
134 case Vehicle.HOVER_EFFICIENCY:
135 // m_hoverEfficiency = pValue;
136 break;
137 case Vehicle.HOVER_HEIGHT:
138 // m_hoverHeight = pValue;
139 break;
140 case Vehicle.HOVER_TIMESCALE:
141 if (pValue < 0.01f) pValue = 0.01f;
142 // m_hoverTimescale = pValue;
143 break;
144 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
145 if (pValue < 0.01f) pValue = 0.01f;
146 // m_linearDeflectionEfficiency = pValue;
147 break;
148 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
149 if (pValue < 0.01f) pValue = 0.01f;
150 // m_linearDeflectionTimescale = pValue;
151 break;
152 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
153 if (pValue < 0.01f) pValue = 0.01f;
154 m_linearMotorDecayTimescale = pValue;
155 break;
156 case Vehicle.LINEAR_MOTOR_TIMESCALE:
157 if (pValue < 0.01f) pValue = 0.01f;
158 m_linearMotorTimescale = pValue;
159 break;
160 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
161 if (pValue < 0.01f) pValue = 0.01f;
162 m_verticalAttractionEfficiency = pValue;
163 break;
164 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
165 if (pValue < 0.01f) pValue = 0.01f;
166 m_verticalAttractionTimescale = pValue;
167 break;
168
169 // These are vector properties but the engine lets you use a single float value to
170 // set all of the components to the same value
171 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
172 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
173 break;
174 case Vehicle.ANGULAR_MOTOR_DIRECTION:
175 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
176 m_angularMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue);
177 break;
178 case Vehicle.LINEAR_FRICTION_TIMESCALE:
179 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
180 break;
181 case Vehicle.LINEAR_MOTOR_DIRECTION:
182 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
183 m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue);
184 break;
185 case Vehicle.LINEAR_MOTOR_OFFSET:
186 // m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
187 break;
188
189 }
190 Reset();
191 }
192
193 internal void ProcessVectorVehicleParam(Vehicle pParam, PhysicsVector pValue)
194 {
195 switch (pParam)
196 {
197 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
198 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
199 break;
200 case Vehicle.ANGULAR_MOTOR_DIRECTION:
201 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
202 m_angularMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
203 break;
204 case Vehicle.LINEAR_FRICTION_TIMESCALE:
205 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
206 break;
207 case Vehicle.LINEAR_MOTOR_DIRECTION:
208 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
209 m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
210 break;
211 case Vehicle.LINEAR_MOTOR_OFFSET:
212 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
213 break;
214 }
215 Reset();
216 }
217
218 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
219 {
220 switch (pParam)
221 {
222 case Vehicle.REFERENCE_FRAME:
223 // m_referenceFrame = pValue;
224 break;
225 }
226 Reset();
227 }
228
229 internal void ProcessTypeChange(Vehicle pType)
230 {
231 if (m_type == Vehicle.TYPE_NONE && pType != Vehicle.TYPE_NONE)
232 {
233 // Activate whatever it is
234 SetDefaultsForType(pType);
235 Reset();
236 }
237 else if (m_type != Vehicle.TYPE_NONE && pType != Vehicle.TYPE_NONE)
238 {
239 // Set properties
240 SetDefaultsForType(pType);
241 // then reset
242 Reset();
243 }
244 else if (m_type != Vehicle.TYPE_NONE && pType == Vehicle.TYPE_NONE)
245 {
246 m_type = pType;
247 Destroy();
248 }
249 }
250
251 internal void Disable()
252 {
253 if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
254 return;
255
256 if (m_aMotor != IntPtr.Zero)
257 {
258
259 }
260
261 }
262
263 internal void Enable(IntPtr pBody, OdeScene pParentScene)
264 {
265 if (m_type == Vehicle.TYPE_NONE)
266 return;
267
268 m_body = pBody;
269 // m_parentScene = pParentScene;
270 if (m_jointGroup == IntPtr.Zero)
271 m_jointGroup = d.JointGroupCreate(3);
272
273 if (pBody != IntPtr.Zero)
274 {
275
276 if (m_lMotor1 == IntPtr.Zero)
277 {
278 d.BodySetAutoDisableFlag(Body, false);
279 m_lMotor1 = d.JointCreateLMotor(pParentScene.world, m_jointGroup);
280 d.JointSetLMotorNumAxes(m_lMotor1, 1);
281 d.JointAttach(m_lMotor1, Body, IntPtr.Zero);
282 }
283
284 if (m_aMotor == IntPtr.Zero)
285 {
286 m_aMotor = d.JointCreateAMotor(pParentScene.world, m_jointGroup);
287 d.JointSetAMotorNumAxes(m_aMotor, 3);
288 d.JointAttach(m_aMotor, Body, IntPtr.Zero);
289 }
290 }
291 }
292
293 internal void Reset()
294 {
295 if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
296 return;
297
298 }
299
300 internal void Destroy()
301 {
302 if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
303 return;
304 if (m_aMotor != IntPtr.Zero)
305 {
306 d.JointDestroy(m_aMotor);
307 }
308 if (m_lMotor1 != IntPtr.Zero)
309 {
310 d.JointDestroy(m_lMotor1);
311 }
312
313 }
314
315 internal void Step(float pTimestep)
316 {
317 if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
318 return;
319 frcount++;
320 if (frcount > 100)
321 frcount = 0;
322
323 VerticalAttractor(pTimestep);
324 LinearMotor(pTimestep);
325
326
327 AngularMotor(pTimestep);
328
329 }
330
331 private void SetDefaultsForType(Vehicle pType)
332 {
333 m_type = pType;
334 switch (pType)
335 {
336 case Vehicle.TYPE_SLED:
337 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
338 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
339 m_linearMotorDirection = Vector3.Zero;
340 m_linearMotorTimescale = 1000;
341 m_linearMotorDecayTimescale = 120;
342 m_angularMotorDirection = Vector3.Zero;
343 m_angularMotorTimescale = 1000;
344 m_angularMotorDecayTimescale = 120;
345 // m_hoverHeight = 0;
346 // m_hoverEfficiency = 10;
347 // m_hoverTimescale = 10;
348 // m_buoyancy = 0;
349 // m_linearDeflectionEfficiency = 1;
350 // m_linearDeflectionTimescale = 1;
351 // m_angularDeflectionEfficiency = 1;
352 // m_angularDeflectionTimescale = 1000;
353 // m_bankingEfficiency = 0;
354 // m_bankingMix = 1;
355 // m_bankingTimescale = 10;
356 // m_referenceFrame = Quaternion.Identity;
357 m_flags &=
358 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
359 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
360 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
361 break;
362 case Vehicle.TYPE_CAR:
363 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
364 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
365 m_linearMotorDirection = Vector3.Zero;
366 m_linearMotorTimescale = 1;
367 m_linearMotorDecayTimescale = 60;
368 m_angularMotorDirection = Vector3.Zero;
369 m_angularMotorTimescale = 1;
370 m_angularMotorDecayTimescale = 0.8f;
371 // m_hoverHeight = 0;
372 // // m_hoverEfficiency = 0;
373 // // m_hoverTimescale = 1000;
374 // // m_buoyancy = 0;
375 // // m_linearDeflectionEfficiency = 1;
376 // // m_linearDeflectionTimescale = 2;
377 // // m_angularDeflectionEfficiency = 0;
378 // m_angularDeflectionTimescale = 10;
379 m_verticalAttractionEfficiency = 1;
380 m_verticalAttractionTimescale = 10;
381 // m_bankingEfficiency = -0.2f;
382 // m_bankingMix = 1;
383 // m_bankingTimescale = 1;
384 // m_referenceFrame = Quaternion.Identity;
385 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
386 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY |
387 VehicleFlag.LIMIT_MOTOR_UP);
388 break;
389 case Vehicle.TYPE_BOAT:
390 m_linearFrictionTimescale = new Vector3(10, 3, 2);
391 m_angularFrictionTimescale = new Vector3(10,10,10);
392 m_linearMotorDirection = Vector3.Zero;
393 m_linearMotorTimescale = 5;
394 m_linearMotorDecayTimescale = 60;
395 m_angularMotorDirection = Vector3.Zero;
396 m_angularMotorTimescale = 4;
397 m_angularMotorDecayTimescale = 4;
398 // m_hoverHeight = 0;
399 // m_hoverEfficiency = 0.5f;
400 // m_hoverTimescale = 2;
401 // m_buoyancy = 1;
402 // m_linearDeflectionEfficiency = 0.5f;
403 // m_linearDeflectionTimescale = 3;
404 // m_angularDeflectionEfficiency = 0.5f;
405 // m_angularDeflectionTimescale = 5;
406 m_verticalAttractionEfficiency = 0.5f;
407 m_verticalAttractionTimescale = 5;
408 // m_bankingEfficiency = -0.3f;
409 // m_bankingMix = 0.8f;
410 // m_bankingTimescale = 1;
411 // m_referenceFrame = Quaternion.Identity;
412 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
413 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_UP_ONLY |
414 VehicleFlag.LIMIT_MOTOR_UP);
415 break;
416 case Vehicle.TYPE_AIRPLANE:
417 m_linearFrictionTimescale = new Vector3(200, 10, 5);
418 m_angularFrictionTimescale = new Vector3(20, 20, 20);
419 m_linearMotorDirection = Vector3.Zero;
420 m_linearMotorTimescale = 2;
421 m_linearMotorDecayTimescale = 60;
422 m_angularMotorDirection = Vector3.Zero;
423 m_angularMotorTimescale = 4;
424 m_angularMotorDecayTimescale = 4;
425 // m_hoverHeight = 0;
426 // m_hoverEfficiency = 0.5f;
427 // m_hoverTimescale = 1000;
428 // m_buoyancy = 0;
429 // m_linearDeflectionEfficiency = 0.5f;
430 // m_linearDeflectionTimescale = 3;
431 // m_angularDeflectionEfficiency = 1;
432 // m_angularDeflectionTimescale = 2;
433 m_verticalAttractionEfficiency = 0.9f;
434 m_verticalAttractionTimescale = 2;
435 // m_bankingEfficiency = 1;
436 // m_bankingMix = 0.7f;
437 // m_bankingTimescale = 2;
438 // m_referenceFrame = Quaternion.Identity;
439 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
440 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
441 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
442 break;
443 case Vehicle.TYPE_BALLOON:
444 m_linearFrictionTimescale = new Vector3(5, 5, 5);
445 m_angularFrictionTimescale = new Vector3(10, 10, 10);
446 m_linearMotorDirection = Vector3.Zero;
447 m_linearMotorTimescale = 5;
448 m_linearMotorDecayTimescale = 60;
449 m_angularMotorDirection = Vector3.Zero;
450 m_angularMotorTimescale = 6;
451 m_angularMotorDecayTimescale = 10;
452 // m_hoverHeight = 5;
453 // m_hoverEfficiency = 0.8f;
454 // m_hoverTimescale = 10;
455 // m_buoyancy = 1;
456 // m_linearDeflectionEfficiency = 0;
457 // m_linearDeflectionTimescale = 5;
458 // m_angularDeflectionEfficiency = 0;
459 // m_angularDeflectionTimescale = 5;
460 m_verticalAttractionEfficiency = 1;
461 m_verticalAttractionTimescale = 1000;
462 // m_bankingEfficiency = 0;
463 // m_bankingMix = 0.7f;
464 // m_bankingTimescale = 5;
465 // m_referenceFrame = Quaternion.Identity;
466 m_flags = (VehicleFlag)0;
467 break;
468
469 }
470 }
471
472 private void VerticalAttractor(float pTimestep)
473 {
474 // The purpose of this routine here is to quickly stabilize the Body while it's popped up in the air.
475 // The amotor needs a few seconds to stabilize so without it, the avatar shoots up sky high when you
476 // change appearance and when you enter the simulator
477 // After this routine is done, the amotor stabilizes much quicker
478 d.Mass objMass;
479 d.BodyGetMass(Body, out objMass);
480 //d.BodyGetS
481
482 d.Vector3 feet;
483 d.Vector3 head;
484 d.BodyGetRelPointPos(m_body, 0.0f, 0.0f, -1.0f, out feet);
485 d.BodyGetRelPointPos(m_body, 0.0f, 0.0f, 1.0f, out head);
486 float posture = head.Z - feet.Z;
487
488 //Console.WriteLine(String.Format("head: <{0},{1},{2}>, feet:<{3},{4},{5}> diff:<{6},{7},{8}>", head.X, head.Y, head.Z, feet.X,
489 // feet.Y, feet.Z, head.X - feet.X, head.Y - feet.Y, head.Z - feet.Z));
490 //Console.WriteLine(String.Format("diff:<{0},{1},{2}>",head.X - feet.X, head.Y - feet.Y, head.Z - feet.Z));
491
492 // restoring force proportional to lack of posture:
493 float servo = (2.5f - posture) * (objMass.mass * m_verticalAttractionEfficiency / (m_verticalAttractionTimescale * pTimestep)) * objMass.mass;
494 d.BodyAddForceAtRelPos(m_body, 0.0f, 0.0f, servo, 0.0f, 0.0f, 1.0f);
495 d.BodyAddForceAtRelPos(m_body, 0.0f, 0.0f, -servo, 0.0f, 0.0f, -1.0f);
496 //d.BodyAddTorque(m_body, (head.X - feet.X) * servo, (head.Y - feet.Y) * servo, (head.Z - feet.Z) * servo);
497 //d.Matrix3 bodyrotation = d.BodyGetRotation(Body);
498 //m_log.Info("[PHYSICSAV]: Rotation: " + bodyrotation.M00 + " : " + bodyrotation.M01 + " : " + bodyrotation.M02 + " : " + bodyrotation.M10 + " : " + bodyrotation.M11 + " : " + bodyrotation.M12 + " : " + bodyrotation.M20 + " : " + bodyrotation.M21 + " : " + bodyrotation.M22);
499 }
500
501 private void LinearMotor(float pTimestep)
502 {
503
504 if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f))
505 {
506
507 Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep);
508 m_lastLinearVelocityVector += (addAmount*10);
509
510 // This will work temporarily, but we really need to compare speed on an axis
511 if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X))
512 m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X;
513 if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y))
514 m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y;
515 if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z))
516 m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z;
517 //Console.WriteLine("add: " + addAmount);
518
519 Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep)));
520 //Console.WriteLine("decay: " + decayfraction);
521
522 m_linearMotorDirection -= m_linearMotorDirection * decayfraction;
523 //Console.WriteLine("actual: " + m_linearMotorDirection);
524 }
525
526 //System.Console.WriteLine(m_linearMotorDirection + " " + m_lastLinearVelocityVector);
527
528 SetLinearMotorProperties();
529
530 Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
531 m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
532
533 //m_linearMotorDirection *= decayamount;
534
535 }
536
537 private void SetLinearMotorProperties()
538 {
539 Vector3 dirNorm = m_lastLinearVelocityVector;
540 dirNorm.Normalize();
541
542 d.Mass objMass;
543 d.BodyGetMass(Body, out objMass);
544 d.Quaternion rot = d.BodyGetQuaternion(Body);
545 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
546 dirNorm *= rotq;
547 if (m_lMotor1 != IntPtr.Zero)
548 {
549
550 d.JointSetLMotorAxis(m_lMotor1, 0, 1, dirNorm.X, dirNorm.Y, dirNorm.Z);
551 d.JointSetLMotorParam(m_lMotor1, (int)dParam.Vel, m_lastLinearVelocityVector.Length());
552
553 d.JointSetLMotorParam(m_lMotor1, (int)dParam.FMax, 35f * objMass.mass);
554 }
555
556 }
557
558 private void AngularMotor(float pTimestep)
559 {
560 if (!m_angularMotorDirection.ApproxEquals(Vector3.Zero, 0.01f))
561 {
562
563 Vector3 addAmount = m_angularMotorDirection / (m_angularMotorTimescale / pTimestep);
564 m_lastAngularVelocityVector += (addAmount * 10);
565
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 //Console.WriteLine("add: " + addAmount);
574
575 Vector3 decayfraction = ((Vector3.One / (m_angularMotorDecayTimescale / pTimestep)));
576 //Console.WriteLine("decay: " + decayfraction);
577
578 m_angularMotorDirection -= m_angularMotorDirection * decayfraction;
579 //Console.WriteLine("actual: " + m_linearMotorDirection);
580 }
581
582 //System.Console.WriteLine(m_linearMotorDirection + " " + m_lastLinearVelocityVector);
583
584 SetAngularMotorProperties();
585
586 Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
587 m_lastAngularVelocityVector -= m_lastAngularVelocityVector * decayamount;
588
589 //m_linearMotorDirection *= decayamount;
590
591 }
592 private void SetAngularMotorProperties()
593 {
594
595
596
597 d.Mass objMass;
598 d.BodyGetMass(Body, out objMass);
599 //d.Quaternion rot = d.BodyGetQuaternion(Body);
600 //Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
601 Vector3 axis0 = Vector3.UnitX;
602 Vector3 axis1 = Vector3.UnitY;
603 Vector3 axis2 = Vector3.UnitZ;
604 //axis0 *= rotq;
605 //axis1 *= rotq;
606 //axis2 *= rotq;
607
608
609
610 if (m_aMotor != IntPtr.Zero)
611 {
612 d.JointSetAMotorAxis(m_aMotor, 0, 1, axis0.X, axis0.Y, axis0.Z);
613 d.JointSetAMotorAxis(m_aMotor, 1, 1, axis1.X, axis1.Y, axis1.Z);
614 d.JointSetAMotorAxis(m_aMotor, 2, 1, axis2.X, axis2.Y, axis2.Z);
615 d.JointSetAMotorParam(m_aMotor, (int)dParam.FMax, 30*objMass.mass);
616 d.JointSetAMotorParam(m_aMotor, (int)dParam.FMax2, 30*objMass.mass);
617 d.JointSetAMotorParam(m_aMotor, (int)dParam.FMax3, 30 * objMass.mass);
618 d.JointSetAMotorParam(m_aMotor, (int)dParam.Vel, m_lastAngularVelocityVector.X);
619 d.JointSetAMotorParam(m_aMotor, (int)dParam.Vel2, m_lastAngularVelocityVector.Y);
620 d.JointSetAMotorParam(m_aMotor, (int)dParam.Vel3, m_lastAngularVelocityVector.Z);
621
622 }
623 }
624
625 }
626}
generated by cgit v1.1 at 2024-12-14 16:54:57 +0000