aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/ChOdePlugin
diff options
context:
space:
mode:
authorKitto Flora2010-03-04 16:33:26 -0500
committerKitto Flora2010-03-04 16:33:26 -0500
commit87590491b52e8ca0211274c17a83482534f845f3 (patch)
tree56a55bbbf6a0203b55ac6336fb3b83fa7771febb /OpenSim/Region/Physics/ChOdePlugin
parentRemove a superfluous array creation (diff)
downloadopensim-SC-87590491b52e8ca0211274c17a83482534f845f3.zip
opensim-SC-87590491b52e8ca0211274c17a83482534f845f3.tar.gz
opensim-SC-87590491b52e8ca0211274c17a83482534f845f3.tar.bz2
opensim-SC-87590491b52e8ca0211274c17a83482534f845f3.tar.xz
Fix Physics angular reference frame.
Diffstat (limited to 'OpenSim/Region/Physics/ChOdePlugin')
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.c_comments630
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs33
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs76
3 files changed, 84 insertions, 655 deletions
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.c_comments b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.c_comments
deleted file mode 100644
index 1060aa6..0000000
--- a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.c_comments
+++ /dev/null
@@ -1,630 +0,0 @@
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
40using System;
41using System.Collections.Generic;
42using System.Reflection;
43using System.Runtime.InteropServices;
44using log4net;
45using OpenMetaverse;
46using Ode.NET;
47using OpenSim.Framework;
48using OpenSim.Region.Physics.Manager;
49
50namespace 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 {
270Console.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/*
616if(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}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
index 55d6945..b3b09e6 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
@@ -116,6 +116,8 @@ namespace OpenSim.Region.Physics.OdePlugin
116 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor 116 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
117// private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity 117// private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity
118 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body 118 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
119
120 private Vector3 m_angularLock = Vector3.One;
119 121
120 //Deflection properties 122 //Deflection properties
121 // private float m_angularDeflectionEfficiency = 0; 123 // private float m_angularDeflectionEfficiency = 0;
@@ -296,6 +298,11 @@ namespace OpenSim.Region.Physics.OdePlugin
296 298
297 }//end ProcessRotationVehicleParam 299 }//end ProcessRotationVehicleParam
298 300
301 internal void SetAngularLock(Vector3 pValue)
302 {
303 m_angularLock = pValue;
304 }
305
299 internal void ProcessFlagsVehicleSet(int flags) 306 internal void ProcessFlagsVehicleSet(int flags)
300 { 307 {
301 m_flags |= (VehicleFlag)flags; 308 m_flags |= (VehicleFlag)flags;
@@ -649,8 +656,13 @@ namespace OpenSim.Region.Physics.OdePlugin
649//if(frcount == 0) Console.WriteLine("MoveAngular "); 656//if(frcount == 0) Console.WriteLine("MoveAngular ");
650 657
651 // Get what the body is doing, this includes 'external' influences 658 // Get what the body is doing, this includes 'external' influences
659 d.Quaternion rot = d.BodyGetQuaternion(Body);
660 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
661 Quaternion irotq = Quaternion.Inverse(rotq);
652 d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body); 662 d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body);
653 Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z); 663 Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z);
664 angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation
665
654//if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel); 666//if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel);
655// Vector3 FrAaccel = m_lastAngularVelocity - angObjectVel; 667// Vector3 FrAaccel = m_lastAngularVelocity - angObjectVel;
656// Vector3 initavel = angObjectVel; 668// Vector3 initavel = angObjectVel;
@@ -694,8 +706,8 @@ namespace OpenSim.Region.Physics.OdePlugin
694 { 706 {
695 float VAservo = 1.0f / (m_verticalAttractionTimescale * pTimestep); 707 float VAservo = 1.0f / (m_verticalAttractionTimescale * pTimestep);
696 // get present body rotation 708 // get present body rotation
697 d.Quaternion rot = d.BodyGetQuaternion(Body); 709// d.Quaternion rot = d.BodyGetQuaternion(Body);
698 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); 710// Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
699 // make a vector pointing up 711 // make a vector pointing up
700 Vector3 verterr = Vector3.Zero; 712 Vector3 verterr = Vector3.Zero;
701 verterr.Z = 1.0f; 713 verterr.Z = 1.0f;
@@ -706,7 +718,7 @@ namespace OpenSim.Region.Physics.OdePlugin
706 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. 718 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
707 719
708 if (verterr.Z < 0.0f) 720 if (verterr.Z < 0.0f)
709 { // Defelction from vertical exceeds 90-degrees. This method will ensure stable return to 721 { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to
710 // vertical, BUT for some reason a z-rotation is imparted to the object. TBI. 722 // vertical, BUT for some reason a z-rotation is imparted to the object. TBI.
711//Console.WriteLine("InvertFlip"); 723//Console.WriteLine("InvertFlip");
712 verterr.X = 2.0f - verterr.X; 724 verterr.X = 2.0f - verterr.X;
@@ -781,9 +793,22 @@ namespace OpenSim.Region.Physics.OdePlugin
781 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero. 793 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
782 } 794 }
783 */ 795 */
796//if(frcount == 0) Console.WriteLine("angularLock {0}", m_angularLock);
797
798 if (!m_angularLock.ApproxEquals(Vector3.One, 0.003f))
799 {
800 if (m_angularLock.X == 0)
801 m_lastAngularVelocity.X = 0f;
802 if (m_angularLock.Y == 0)
803 m_lastAngularVelocity.Y = 0f;
804 if (m_angularLock.Z == 0)
805 m_lastAngularVelocity.Z = 0f;
806 }
784 // Apply to the body 807 // Apply to the body
785// Vector3 aInc = m_lastAngularVelocity - initavel; 808// Vector3 aInc = m_lastAngularVelocity - initavel;
786//if(frcount == 0) Console.WriteLine("Inc {0}", aInc); 809//if(frcount == 0) Console.WriteLine("Inc {0}", aInc);
810 m_lastAngularVelocity = m_lastAngularVelocity * rotq; // ================ Converts to WORLD rotation
811
787 d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); 812 d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
788//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity); 813//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity);
789 814
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
index 8502aef..fb6cb55 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -85,8 +85,10 @@ namespace OpenSim.Region.Physics.OdePlugin
85 private Vector3 m_taintVelocity; 85 private Vector3 m_taintVelocity;
86 private Vector3 m_taintTorque; 86 private Vector3 m_taintTorque;
87 private Quaternion m_taintrot; 87 private Quaternion m_taintrot;
88 private Vector3 m_angularlock = Vector3.One; 88 private Vector3 m_angularlock = Vector3.One; // Current setting
89 private Vector3 m_taintAngularLock = Vector3.One; 89 private Vector3 m_taintAngularLock = Vector3.One; // Request from LSL
90
91
90 private IntPtr Amotor = IntPtr.Zero; 92 private IntPtr Amotor = IntPtr.Zero;
91 93
92 private Vector3 m_PIDTarget; 94 private Vector3 m_PIDTarget;
@@ -405,10 +407,10 @@ namespace OpenSim.Region.Physics.OdePlugin
405 m_disabled = false; 407 m_disabled = false;
406 408
407 // The body doesn't already have a finite rotation mode set here 409 // The body doesn't already have a finite rotation mode set here
408 if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0.0f)) && _parent == null) 410 /* ### if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0.0f)) && _parent == null)
409 { 411 {
410 createAMotor(m_angularlock); 412 createAMotor(m_angularlock);
411 } 413 } */
412 if (m_vehicle.Type != Vehicle.TYPE_NONE) 414 if (m_vehicle.Type != Vehicle.TYPE_NONE)
413 { 415 {
414 m_vehicle.Enable(Body, _parent_scene); 416 m_vehicle.Enable(Body, _parent_scene);
@@ -958,8 +960,10 @@ namespace OpenSim.Region.Physics.OdePlugin
958 960
959 if (m_taintCollidesWater != m_collidesWater) 961 if (m_taintCollidesWater != m_collidesWater)
960 changefloatonwater(timestep); 962 changefloatonwater(timestep);
961 963
964 // ##*
962 if (!m_angularlock.ApproxEquals(m_taintAngularLock,0f)) 965 if (!m_angularlock.ApproxEquals(m_taintAngularLock,0f))
966//Console.WriteLine("ALchange req {0} is {1}", m_taintAngularLock, m_angularlock);
963 changeAngularLock(timestep); 967 changeAngularLock(timestep);
964 968
965 } 969 }
@@ -970,15 +974,21 @@ namespace OpenSim.Region.Physics.OdePlugin
970 } 974 }
971 975
972 976
973 private void changeAngularLock(float timestep) 977 private void changeAngularLock(float timestep) // ##*
974 { 978 {
975 // do we have a Physical object? 979 // do we have a Physical object?
976 if (Body != IntPtr.Zero) 980// if (Body != IntPtr.Zero)
977 { 981// {
978 //Check that we have a Parent 982 //Check that we have a Parent
979 //If we have a parent then we're not authorative here 983 //If we have a parent then we're not authorative here
980 if (_parent == null) 984 if (_parent == null)
981 { 985 {
986//Console.WriteLine("Alock changed to {0}", m_taintAngularLock);
987 m_angularlock = m_taintAngularLock;
988 m_vehicle.SetAngularLock(m_angularlock);
989
990
991/*
982 if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f)) 992 if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f))
983 { 993 {
984 //d.BodySetFiniteRotationMode(Body, 0); 994 //d.BodySetFiniteRotationMode(Body, 0);
@@ -997,7 +1007,9 @@ namespace OpenSim.Region.Physics.OdePlugin
997 } 1007 }
998 // Store this for later in case we get turned into a separate body 1008 // Store this for later in case we get turned into a separate body
999 m_angularlock = m_taintAngularLock; 1009 m_angularlock = m_taintAngularLock;
1000 1010 m_vehicle.SetAngularLock(m_angularlock);
1011 } */
1012 }
1001 } 1013 }
1002 1014
1003 private void changelink(float timestep) 1015 private void changelink(float timestep)
@@ -1140,10 +1152,10 @@ namespace OpenSim.Region.Physics.OdePlugin
1140 prm.m_disabled = false; 1152 prm.m_disabled = false;
1141 1153
1142 // The body doesn't already have a finite rotation mode set here 1154 // The body doesn't already have a finite rotation mode set here
1143 if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null) 1155 /* ### if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
1144 { 1156 {
1145 prm.createAMotor(m_angularlock); 1157 prm.createAMotor(m_angularlock);
1146 } 1158 } */
1147 prm.Body = Body; 1159 prm.Body = Body;
1148 _parent_scene.addActivePrim(prm); 1160 _parent_scene.addActivePrim(prm);
1149 } 1161 }
@@ -1183,10 +1195,10 @@ namespace OpenSim.Region.Physics.OdePlugin
1183 m_disabled = false; 1195 m_disabled = false;
1184 1196
1185 // The body doesn't already have a finite rotation mode set here 1197 // The body doesn't already have a finite rotation mode set here
1186 if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null) 1198 /* ### if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
1187 { 1199 {
1188 createAMotor(m_angularlock); 1200 createAMotor(m_angularlock);
1189 } 1201 } */
1190 d.BodySetPosition(Body, Position.X, Position.Y, Position.Z); 1202 d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
1191 if (m_vehicle.Type != Vehicle.TYPE_NONE) m_vehicle.Enable(Body, _parent_scene); 1203 if (m_vehicle.Type != Vehicle.TYPE_NONE) m_vehicle.Enable(Body, _parent_scene);
1192 _parent_scene.addActivePrim(this); 1204 _parent_scene.addActivePrim(this);
@@ -1600,14 +1612,17 @@ Console.WriteLine(" JointCreateFixed");
1600 { 1612 {
1601 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 1613 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
1602 // NON-'VEHICLES' are dealt with here 1614 // NON-'VEHICLES' are dealt with here
1603 if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f)) 1615 // m_angularlock = <1,1,1> means no lock. a 0 on axis means locked.
1616
1617// NB this may be wrong - may lock global axis! Should be LOCAL axis!
1618 if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.One, 0.003f))
1604 { 1619 {
1605 d.Vector3 avel2 = d.BodyGetAngularVel(Body); 1620 d.Vector3 avel2 = d.BodyGetAngularVel(Body);
1606 if (m_angularlock.X == 1) 1621 if (m_angularlock.X == 0)
1607 avel2.X = 0; 1622 avel2.X = 0;
1608 if (m_angularlock.Y == 1) 1623 if (m_angularlock.Y == 0)
1609 avel2.Y = 0; 1624 avel2.Y = 0;
1610 if (m_angularlock.Z == 1) 1625 if (m_angularlock.Z == 0)
1611 avel2.Z = 0; 1626 avel2.Z = 0;
1612 d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z); 1627 d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
1613 } 1628 }
@@ -1808,6 +1823,17 @@ Console.WriteLine(" JointCreateFixed");
1808 // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z); 1823 // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z);
1809 RLAservo = timestep / m_APIDStrength * scaler; 1824 RLAservo = timestep / m_APIDStrength * scaler;
1810 rotforce = rotforce * RLAservo * diff_angle ; 1825 rotforce = rotforce * RLAservo * diff_angle ;
1826
1827 if (m_angularlock.X == 0)
1828 rotforce.X = 0;
1829 if (m_angularlock.Y == 0)
1830 rotforce.Y = 0;
1831 if (m_angularlock.Z == 0)
1832 rotforce.Z = 0;
1833
1834
1835
1836
1811 d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z); 1837 d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z);
1812//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); 1838//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo);
1813 } 1839 }
@@ -1878,11 +1904,11 @@ Console.WriteLine(" JointCreateFixed");
1878 { 1904 {
1879 // KF: If this is a root prim do BodySet 1905 // KF: If this is a root prim do BodySet
1880 d.BodySetQuaternion(Body, ref myrot); 1906 d.BodySetQuaternion(Body, ref myrot);
1881 if (m_isphysical) 1907 /* ### if (m_isphysical)
1882 { 1908 {
1883 if (!m_angularlock.ApproxEquals(Vector3.One, 0f)) 1909 if (!m_angularlock.ApproxEquals(Vector3.One, 0f))
1884 createAMotor(m_angularlock); 1910 createAMotor(m_angularlock);
1885 } 1911 } */
1886 } 1912 }
1887 else 1913 else
1888 { 1914 {
@@ -2508,7 +2534,14 @@ Console.WriteLine(" JointCreateFixed");
2508 } 2534 }
2509 } 2535 }
2510 } 2536 }
2511 2537/*
2538 public Vector3 AngularLock
2539 {
2540 get { return m_angularlock; }
2541 set { }
2542 }
2543
2544*/
2512 public override float CollisionScore 2545 public override float CollisionScore
2513 { 2546 {
2514 get { return m_collisionscore; } 2547 get { return m_collisionscore; }
@@ -2914,8 +2947,9 @@ Console.WriteLine(" JointCreateFixed");
2914 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } 2947 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
2915 public override float PIDHoverTau { set { m_PIDHoverTau = value; } } 2948 public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
2916 2949
2917 private void createAMotor(Vector3 axis) 2950 private void createAMotor(Vector3 axis) // ##*
2918 { 2951 {
2952Console.WriteLine(" createAMotor called! ----------------------------");
2919 if (Body == IntPtr.Zero) 2953 if (Body == IntPtr.Zero)
2920 return; 2954 return;
2921 2955
generated by cgit v1.1 at 2024-12-13 21:45:14 +0000