aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/OdePlugin
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--OpenSim/Region/Physics/OdePlugin/ODEDynamics.c_comments (renamed from OpenSim/Region/Physics/OdePlugin/ODEVehicleSettings.cs)558
-rw-r--r--OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs658
-rw-r--r--OpenSim/Region/Physics/OdePlugin/ODEPrim.cs589
-rw-r--r--OpenSim/Region/Physics/OdePlugin/OdePlugin.cs40
4 files changed, 1289 insertions, 556 deletions
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEVehicleSettings.cs b/OpenSim/Region/Physics/OdePlugin/ODEDynamics.c_comments
index a547c3e..1060aa6 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEVehicleSettings.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEDynamics.c_comments
@@ -1,4 +1,16 @@
1/* 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 *
2 * Copyright (c) Contributors, http://opensimulator.org/ 14 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders. 15 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 * 16 *
@@ -37,10 +49,10 @@ using OpenSim.Region.Physics.Manager;
37 49
38namespace OpenSim.Region.Physics.OdePlugin 50namespace OpenSim.Region.Physics.OdePlugin
39{ 51{
40 public class ODEVehicleSettings 52 public class ODEDynamics
41 { 53 {
42 public Vehicle Type 54 public Vehicle Type
43 { 55 {
44 get { return m_type; } 56 get { return m_type; }
45 } 57 }
46 58
@@ -49,49 +61,71 @@ namespace OpenSim.Region.Physics.OdePlugin
49 get { return m_body; } 61 get { return m_body; }
50 } 62 }
51 63
52 private int frcount = 0; 64 private int frcount = 0; // Used to limit dynamics debug output to
53 // private float frmod = 3.0f; 65 // every 100th frame
54 66
55 private Vehicle m_type = Vehicle.TYPE_NONE;
56 // private OdeScene m_parentScene = null; 67 // private OdeScene m_parentScene = null;
57 private IntPtr m_body = IntPtr.Zero; 68 private IntPtr m_body = IntPtr.Zero;
58 private IntPtr m_jointGroup = IntPtr.Zero; 69 private IntPtr m_jointGroup = IntPtr.Zero;
59 private IntPtr m_aMotor = IntPtr.Zero; 70 private IntPtr m_aMotor = IntPtr.Zero;
60 private IntPtr m_lMotor1 = IntPtr.Zero; 71
61 // private IntPtr m_lMotor2 = IntPtr.Zero;
62 // private IntPtr m_lMotor3 = IntPtr.Zero;
63 72
64 // Vehicle properties 73 // Vehicle properties
65 // private Quaternion m_referenceFrame = Quaternion.Identity; 74 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
66 private Vector3 m_angularFrictionTimescale = Vector3.Zero; 75 // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
67 private Vector3 m_angularMotorDirection = Vector3.Zero; 76 private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings:
68 private Vector3 m_angularMotorDirectionLASTSET = Vector3.Zero; 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
69 private Vector3 m_linearFrictionTimescale = Vector3.Zero; 89 private Vector3 m_linearFrictionTimescale = Vector3.Zero;
70 private Vector3 m_linearMotorDirection = Vector3.Zero; 90 private float m_linearMotorDecayTimescale = 0;
71 private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; 91 private float m_linearMotorTimescale = 0;
92 private Vector3 m_lastLinearVelocityVector = Vector3.Zero;
93 // private bool m_LinearMotorSetLastFrame = false;
72 // private Vector3 m_linearMotorOffset = Vector3.Zero; 94 // private Vector3 m_linearMotorOffset = Vector3.Zero;
73 // private float m_angularDeflectionEfficiency = 0; 95
74 // private float m_angularDeflectionTimescale = 0; 96 //Angular properties
97 private Vector3 m_angularMotorDirection = Vector3.Zero;
98 private Vector3 m_angularMotorDirectionLASTSET = Vector3.Zero;
99 private Vector3 m_angularFrictionTimescale = Vector3.Zero;
75 private float m_angularMotorDecayTimescale = 0; 100 private float m_angularMotorDecayTimescale = 0;
76 private float m_angularMotorTimescale = 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
77 // private float m_bankingEfficiency = 0; 111 // private float m_bankingEfficiency = 0;
78 // private float m_bankingMix = 0; 112 // private float m_bankingMix = 0;
79 // private float m_bankingTimescale = 0; 113 // private float m_bankingTimescale = 0;
80 // private float m_buoyancy = 0; 114
81 // private float m_hoverHeight = 0; 115 //Hover and Buoyancy properties
82 // private float m_hoverEfficiency = 0; 116 private float m_VhoverHeight = 0f;
83 // private float m_hoverTimescale = 0; 117 private float m_VhoverEfficiency = 0f;
84 // private float m_linearDeflectionEfficiency = 0; 118 private float m_VhoverTimescale = 0f;
85 // private float m_linearDeflectionTimescale = 0; 119 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
86 private float m_linearMotorDecayTimescale = 0; 120 private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle.
87 private float m_linearMotorTimescale = 0; 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
88 private float m_verticalAttractionEfficiency = 0; 126 private float m_verticalAttractionEfficiency = 0;
89 private float m_verticalAttractionTimescale = 0; 127 private float m_verticalAttractionTimescale = 0;
90 private Vector3 m_lastLinearVelocityVector = Vector3.Zero; 128
91 private Vector3 m_lastAngularVelocityVector = Vector3.Zero;
92 private VehicleFlag m_flags = (VehicleFlag) 0;
93
94 // private bool m_LinearMotorSetLastFrame = false;
95 129
96 130
97 131
@@ -129,17 +163,21 @@ namespace OpenSim.Region.Physics.OdePlugin
129 // m_bankingTimescale = pValue; 163 // m_bankingTimescale = pValue;
130 break; 164 break;
131 case Vehicle.BUOYANCY: 165 case Vehicle.BUOYANCY:
132 // m_buoyancy = pValue; 166 if (pValue < -1f) pValue = -1f;
167 if (pValue > 1f) pValue = 1f;
168 m_VehicleBuoyancy = pValue;
133 break; 169 break;
134 case Vehicle.HOVER_EFFICIENCY: 170 case Vehicle.HOVER_EFFICIENCY:
135 // m_hoverEfficiency = pValue; 171 if (pValue < 0f) pValue = 0f;
172 if (pValue > 1f) pValue = 1f;
173 m_VhoverEfficiency = pValue;
136 break; 174 break;
137 case Vehicle.HOVER_HEIGHT: 175 case Vehicle.HOVER_HEIGHT:
138 // m_hoverHeight = pValue; 176 m_VhoverHeight = pValue;
139 break; 177 break;
140 case Vehicle.HOVER_TIMESCALE: 178 case Vehicle.HOVER_TIMESCALE:
141 if (pValue < 0.01f) pValue = 0.01f; 179 if (pValue < 0.01f) pValue = 0.01f;
142 // m_hoverTimescale = pValue; 180 m_VhoverTimescale = pValue;
143 break; 181 break;
144 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: 182 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
145 if (pValue < 0.01f) pValue = 0.01f; 183 if (pValue < 0.01f) pValue = 0.01f;
@@ -158,7 +196,8 @@ namespace OpenSim.Region.Physics.OdePlugin
158 m_linearMotorTimescale = pValue; 196 m_linearMotorTimescale = pValue;
159 break; 197 break;
160 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: 198 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
161 if (pValue < 0.01f) pValue = 0.01f; 199 if (pValue < 0.0f) pValue = 0.0f;
200 if (pValue > 1.0f) pValue = 1.0f;
162 m_verticalAttractionEfficiency = pValue; 201 m_verticalAttractionEfficiency = pValue;
163 break; 202 break;
164 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: 203 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
@@ -187,8 +226,8 @@ namespace OpenSim.Region.Physics.OdePlugin
187 break; 226 break;
188 227
189 } 228 }
190 Reset(); 229
191 } 230 }//end ProcessFloatVehicleParam
192 231
193 internal void ProcessVectorVehicleParam(Vehicle pParam, PhysicsVector pValue) 232 internal void ProcessVectorVehicleParam(Vehicle pParam, PhysicsVector pValue)
194 { 233 {
@@ -212,8 +251,8 @@ namespace OpenSim.Region.Physics.OdePlugin
212 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); 251 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
213 break; 252 break;
214 } 253 }
215 Reset(); 254
216 } 255 }//end ProcessVectorVehicleParam
217 256
218 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) 257 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
219 { 258 {
@@ -223,113 +262,14 @@ namespace OpenSim.Region.Physics.OdePlugin
223 // m_referenceFrame = pValue; 262 // m_referenceFrame = pValue;
224 break; 263 break;
225 } 264 }
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 265
298 } 266 }//end ProcessRotationVehicleParam
299 267
300 internal void Destroy() 268 internal void ProcessTypeChange(Vehicle pType)
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 { 269 {
270Console.WriteLine("ProcessTypeChange to " + pType);
271
272 // Set Defaults For Type
333 m_type = pType; 273 m_type = pType;
334 switch (pType) 274 switch (pType)
335 { 275 {
@@ -342,10 +282,10 @@ namespace OpenSim.Region.Physics.OdePlugin
342 m_angularMotorDirection = Vector3.Zero; 282 m_angularMotorDirection = Vector3.Zero;
343 m_angularMotorTimescale = 1000; 283 m_angularMotorTimescale = 1000;
344 m_angularMotorDecayTimescale = 120; 284 m_angularMotorDecayTimescale = 120;
345 // m_hoverHeight = 0; 285 m_VhoverHeight = 0;
346 // m_hoverEfficiency = 10; 286 m_VhoverEfficiency = 1;
347 // m_hoverTimescale = 10; 287 m_VhoverTimescale = 10;
348 // m_buoyancy = 0; 288 m_VehicleBuoyancy = 0;
349 // m_linearDeflectionEfficiency = 1; 289 // m_linearDeflectionEfficiency = 1;
350 // m_linearDeflectionTimescale = 1; 290 // m_linearDeflectionTimescale = 1;
351 // m_angularDeflectionEfficiency = 1; 291 // m_angularDeflectionEfficiency = 1;
@@ -368,10 +308,10 @@ namespace OpenSim.Region.Physics.OdePlugin
368 m_angularMotorDirection = Vector3.Zero; 308 m_angularMotorDirection = Vector3.Zero;
369 m_angularMotorTimescale = 1; 309 m_angularMotorTimescale = 1;
370 m_angularMotorDecayTimescale = 0.8f; 310 m_angularMotorDecayTimescale = 0.8f;
371 // m_hoverHeight = 0; 311 m_VhoverHeight = 0;
372 // // m_hoverEfficiency = 0; 312 m_VhoverEfficiency = 0;
373 // // m_hoverTimescale = 1000; 313 m_VhoverTimescale = 1000;
374 // // m_buoyancy = 0; 314 m_VehicleBuoyancy = 0;
375 // // m_linearDeflectionEfficiency = 1; 315 // // m_linearDeflectionEfficiency = 1;
376 // // m_linearDeflectionTimescale = 2; 316 // // m_linearDeflectionTimescale = 2;
377 // // m_angularDeflectionEfficiency = 0; 317 // // m_angularDeflectionEfficiency = 0;
@@ -395,10 +335,10 @@ namespace OpenSim.Region.Physics.OdePlugin
395 m_angularMotorDirection = Vector3.Zero; 335 m_angularMotorDirection = Vector3.Zero;
396 m_angularMotorTimescale = 4; 336 m_angularMotorTimescale = 4;
397 m_angularMotorDecayTimescale = 4; 337 m_angularMotorDecayTimescale = 4;
398 // m_hoverHeight = 0; 338 m_VhoverHeight = 0;
399 // m_hoverEfficiency = 0.5f; 339 m_VhoverEfficiency = 0.5f;
400 // m_hoverTimescale = 2; 340 m_VhoverTimescale = 2;
401 // m_buoyancy = 1; 341 m_VehicleBuoyancy = 1;
402 // m_linearDeflectionEfficiency = 0.5f; 342 // m_linearDeflectionEfficiency = 0.5f;
403 // m_linearDeflectionTimescale = 3; 343 // m_linearDeflectionTimescale = 3;
404 // m_angularDeflectionEfficiency = 0.5f; 344 // m_angularDeflectionEfficiency = 0.5f;
@@ -409,8 +349,9 @@ namespace OpenSim.Region.Physics.OdePlugin
409 // m_bankingMix = 0.8f; 349 // m_bankingMix = 0.8f;
410 // m_bankingTimescale = 1; 350 // m_bankingTimescale = 1;
411 // m_referenceFrame = Quaternion.Identity; 351 // m_referenceFrame = Quaternion.Identity;
412 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); 352 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY |
413 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_UP_ONLY | 353 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
354 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY |
414 VehicleFlag.LIMIT_MOTOR_UP); 355 VehicleFlag.LIMIT_MOTOR_UP);
415 break; 356 break;
416 case Vehicle.TYPE_AIRPLANE: 357 case Vehicle.TYPE_AIRPLANE:
@@ -422,10 +363,10 @@ namespace OpenSim.Region.Physics.OdePlugin
422 m_angularMotorDirection = Vector3.Zero; 363 m_angularMotorDirection = Vector3.Zero;
423 m_angularMotorTimescale = 4; 364 m_angularMotorTimescale = 4;
424 m_angularMotorDecayTimescale = 4; 365 m_angularMotorDecayTimescale = 4;
425 // m_hoverHeight = 0; 366 m_VhoverHeight = 0;
426 // m_hoverEfficiency = 0.5f; 367 m_VhoverEfficiency = 0.5f;
427 // m_hoverTimescale = 1000; 368 m_VhoverTimescale = 1000;
428 // m_buoyancy = 0; 369 m_VehicleBuoyancy = 0;
429 // m_linearDeflectionEfficiency = 0.5f; 370 // m_linearDeflectionEfficiency = 0.5f;
430 // m_linearDeflectionTimescale = 3; 371 // m_linearDeflectionTimescale = 3;
431 // m_angularDeflectionEfficiency = 1; 372 // m_angularDeflectionEfficiency = 1;
@@ -449,10 +390,10 @@ namespace OpenSim.Region.Physics.OdePlugin
449 m_angularMotorDirection = Vector3.Zero; 390 m_angularMotorDirection = Vector3.Zero;
450 m_angularMotorTimescale = 6; 391 m_angularMotorTimescale = 6;
451 m_angularMotorDecayTimescale = 10; 392 m_angularMotorDecayTimescale = 10;
452 // m_hoverHeight = 5; 393 m_VhoverHeight = 5;
453 // m_hoverEfficiency = 0.8f; 394 m_VhoverEfficiency = 0.8f;
454 // m_hoverTimescale = 10; 395 m_VhoverTimescale = 10;
455 // m_buoyancy = 1; 396 m_VehicleBuoyancy = 1;
456 // m_linearDeflectionEfficiency = 0; 397 // m_linearDeflectionEfficiency = 0;
457 // m_linearDeflectionTimescale = 5; 398 // m_linearDeflectionTimescale = 5;
458 // m_angularDeflectionEfficiency = 0; 399 // m_angularDeflectionEfficiency = 0;
@@ -463,106 +404,165 @@ namespace OpenSim.Region.Physics.OdePlugin
463 // m_bankingMix = 0.7f; 404 // m_bankingMix = 0.7f;
464 // m_bankingTimescale = 5; 405 // m_bankingTimescale = 5;
465 // m_referenceFrame = Quaternion.Identity; 406 // m_referenceFrame = Quaternion.Identity;
466 m_flags = (VehicleFlag)0; 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);
467 break; 410 break;
468 411
469 } 412 }
470 } 413 }//end SetDefaultsForType
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 414
482 d.Vector3 feet; 415 internal void Enable(IntPtr pBody, OdeScene pParentScene)
483 d.Vector3 head; 416 {
484 d.BodyGetRelPointPos(m_body, 0.0f, 0.0f, -1.0f, out feet); 417//Console.WriteLine("Enable m_type=" + m_type + " m_VehicleBuoyancy=" + m_VehicleBuoyancy);
485 d.BodyGetRelPointPos(m_body, 0.0f, 0.0f, 1.0f, out head); 418 if (m_type == Vehicle.TYPE_NONE)
486 float posture = head.Z - feet.Z; 419 return;
487 420
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, 421 m_body = pBody;
489 // feet.Y, feet.Z, head.X - feet.X, head.Y - feet.Y, head.Z - feet.Z)); 422 //KF: This used to set up the linear and angular joints
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 } 423 }
500 424
501 private void LinearMotor(float pTimestep) 425 internal void Step(float pTimestep, OdeScene pParentScene)
502 { 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;
503 432
504 if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) 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
505 { 440 {
506 441 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
507 Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep);
508 m_lastLinearVelocityVector += (addAmount*10);
509 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
510 // This will work temporarily, but we really need to compare speed on an axis 447 // This will work temporarily, but we really need to compare speed on an axis
448 // KF: Limit body velocity to applied velocity?
511 if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X)) 449 if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X))
512 m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X; 450 m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X;
513 if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y)) 451 if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y))
514 m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y; 452 m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y;
515 if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z)) 453 if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z))
516 m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z; 454 m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z;
517 //Console.WriteLine("add: " + addAmount); 455
518 456 // decay applied velocity
519 Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep))); 457 Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep)));
520 //Console.WriteLine("decay: " + decayfraction); 458 //Console.WriteLine("decay: " + decayfraction);
521
522 m_linearMotorDirection -= m_linearMotorDirection * decayfraction; 459 m_linearMotorDirection -= m_linearMotorDirection * decayfraction;
523 //Console.WriteLine("actual: " + m_linearMotorDirection); 460 //Console.WriteLine("actual: " + m_linearMotorDirection);
524 } 461 }
525 462 else
526 //System.Console.WriteLine(m_linearMotorDirection + " " + m_lastLinearVelocityVector); 463 { // requested is not significant
527 464 // if what remains of applied is small, zero it.
528 SetLinearMotorProperties(); 465 if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
529 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
530 Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep); 546 Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
531 m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount; 547 m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
532 548 } // end MoveLinear()
533 //m_linearMotorDirection *= decayamount; 549
534 550 private void MoveAngular(float pTimestep)
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 { 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
560 if (!m_angularMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) 557 if (!m_angularMotorDirection.ApproxEquals(Vector3.Zero, 0.01f))
561 { 558 {
562 559 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
560 // ramp up to new value
563 Vector3 addAmount = m_angularMotorDirection / (m_angularMotorTimescale / pTimestep); 561 Vector3 addAmount = m_angularMotorDirection / (m_angularMotorTimescale / pTimestep);
564 m_lastAngularVelocityVector += (addAmount * 10); 562 m_lastAngularVelocityVector += (addAmount * 10f);
563//if(frcount == 0) Console.WriteLine("add: " + addAmount);
565 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 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)) 567 if (Math.Abs(m_lastAngularVelocityVector.X) > Math.Abs(m_angularMotorDirectionLASTSET.X))
568 m_lastAngularVelocityVector.X = m_angularMotorDirectionLASTSET.X; 568 m_lastAngularVelocityVector.X = m_angularMotorDirectionLASTSET.X;
@@ -570,57 +570,61 @@ namespace OpenSim.Region.Physics.OdePlugin
570 m_lastAngularVelocityVector.Y = m_angularMotorDirectionLASTSET.Y; 570 m_lastAngularVelocityVector.Y = m_angularMotorDirectionLASTSET.Y;
571 if (Math.Abs(m_lastAngularVelocityVector.Z) > Math.Abs(m_angularMotorDirectionLASTSET.Z)) 571 if (Math.Abs(m_lastAngularVelocityVector.Z) > Math.Abs(m_angularMotorDirectionLASTSET.Z))
572 m_lastAngularVelocityVector.Z = m_angularMotorDirectionLASTSET.Z; 572 m_lastAngularVelocityVector.Z = m_angularMotorDirectionLASTSET.Z;
573 //Console.WriteLine("add: " + addAmount);
574 573
574 // decay the requested value
575 Vector3 decayfraction = ((Vector3.One / (m_angularMotorDecayTimescale / pTimestep))); 575 Vector3 decayfraction = ((Vector3.One / (m_angularMotorDecayTimescale / pTimestep)));
576 //Console.WriteLine("decay: " + decayfraction); 576 //Console.WriteLine("decay: " + decayfraction);
577
578 m_angularMotorDirection -= m_angularMotorDirection * decayfraction; 577 m_angularMotorDirection -= m_angularMotorDirection * decayfraction;
579 //Console.WriteLine("actual: " + m_linearMotorDirection); 578 //Console.WriteLine("actual: " + m_linearMotorDirection);
580 } 579 }
581 580 // KF: m_lastAngularVelocityVector is rotational speed in rad/sec ?
582 //System.Console.WriteLine(m_linearMotorDirection + " " + m_lastLinearVelocityVector); 581
583 582 // Vertical attractor section
584 SetAngularMotorProperties(); 583
585 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
586 Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep); 625 Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
587 m_lastAngularVelocityVector -= m_lastAngularVelocityVector * decayamount; 626 m_lastAngularVelocityVector -= m_lastAngularVelocityVector * decayamount;
588 627
589 //m_linearMotorDirection *= decayamount; 628 } //end MoveAngular
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 } 629 }
626} 630}
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs
new file mode 100644
index 0000000..467eba0
--- /dev/null
+++ b/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs
@@ -0,0 +1,658 @@
1/*
2 * Revised Aug, Sept 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; // angular velocity requested by LSL motor
98 private int m_angularMotorApply = 0; // application frame counter
99 private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity
100 private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate
101 private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate
102 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
103 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
104 // private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body
105
106 //Deflection properties
107 // private float m_angularDeflectionEfficiency = 0;
108 // private float m_angularDeflectionTimescale = 0;
109 // private float m_linearDeflectionEfficiency = 0;
110 // private float m_linearDeflectionTimescale = 0;
111
112 //Banking properties
113 // private float m_bankingEfficiency = 0;
114 // private float m_bankingMix = 0;
115 // private float m_bankingTimescale = 0;
116
117 //Hover and Buoyancy properties
118 private float m_VhoverHeight = 0f;
119 private float m_VhoverEfficiency = 0f;
120 private float m_VhoverTimescale = 0f;
121 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
122 private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle.
123 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
124 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
125 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
126
127 //Attractor properties
128 private float m_verticalAttractionEfficiency = 1.0f; // damped
129 private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor.
130
131
132
133
134
135 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
136 {
137 switch (pParam)
138 {
139 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
140 if (pValue < 0.01f) pValue = 0.01f;
141 // m_angularDeflectionEfficiency = pValue;
142 break;
143 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
144 if (pValue < 0.01f) pValue = 0.01f;
145 // m_angularDeflectionTimescale = pValue;
146 break;
147 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
148 if (pValue < 0.01f) pValue = 0.01f;
149 m_angularMotorDecayTimescale = pValue;
150 break;
151 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
152 if (pValue < 0.01f) pValue = 0.01f;
153 m_angularMotorTimescale = pValue;
154 break;
155 case Vehicle.BANKING_EFFICIENCY:
156 if (pValue < 0.01f) pValue = 0.01f;
157 // m_bankingEfficiency = pValue;
158 break;
159 case Vehicle.BANKING_MIX:
160 if (pValue < 0.01f) pValue = 0.01f;
161 // m_bankingMix = pValue;
162 break;
163 case Vehicle.BANKING_TIMESCALE:
164 if (pValue < 0.01f) pValue = 0.01f;
165 // m_bankingTimescale = pValue;
166 break;
167 case Vehicle.BUOYANCY:
168 if (pValue < -1f) pValue = -1f;
169 if (pValue > 1f) pValue = 1f;
170 m_VehicleBuoyancy = pValue;
171 break;
172 case Vehicle.HOVER_EFFICIENCY:
173 if (pValue < 0f) pValue = 0f;
174 if (pValue > 1f) pValue = 1f;
175 m_VhoverEfficiency = pValue;
176 break;
177 case Vehicle.HOVER_HEIGHT:
178 m_VhoverHeight = pValue;
179 break;
180 case Vehicle.HOVER_TIMESCALE:
181 if (pValue < 0.01f) pValue = 0.01f;
182 m_VhoverTimescale = pValue;
183 break;
184 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
185 if (pValue < 0.01f) pValue = 0.01f;
186 // m_linearDeflectionEfficiency = pValue;
187 break;
188 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
189 if (pValue < 0.01f) pValue = 0.01f;
190 // m_linearDeflectionTimescale = pValue;
191 break;
192 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
193 if (pValue < 0.01f) pValue = 0.01f;
194 m_linearMotorDecayTimescale = pValue;
195 break;
196 case Vehicle.LINEAR_MOTOR_TIMESCALE:
197 if (pValue < 0.01f) pValue = 0.01f;
198 m_linearMotorTimescale = pValue;
199 break;
200 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
201 if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable
202 if (pValue > 1.0f) pValue = 1.0f;
203 m_verticalAttractionEfficiency = pValue;
204 break;
205 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
206 if (pValue < 0.01f) pValue = 0.01f;
207 m_verticalAttractionTimescale = pValue;
208 break;
209
210 // These are vector properties but the engine lets you use a single float value to
211 // set all of the components to the same value
212 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
213 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
214 break;
215 case Vehicle.ANGULAR_MOTOR_DIRECTION:
216 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
217 m_angularMotorApply = 10;
218 break;
219 case Vehicle.LINEAR_FRICTION_TIMESCALE:
220 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
221 break;
222 case Vehicle.LINEAR_MOTOR_DIRECTION:
223 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
224 m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue);
225 break;
226 case Vehicle.LINEAR_MOTOR_OFFSET:
227 // m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
228 break;
229
230 }
231
232 }//end ProcessFloatVehicleParam
233
234 internal void ProcessVectorVehicleParam(Vehicle pParam, PhysicsVector pValue)
235 {
236 switch (pParam)
237 {
238 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
239 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
240 break;
241 case Vehicle.ANGULAR_MOTOR_DIRECTION:
242 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
243 // Limit requested angular speed to 2 rps= 4 pi rads/sec
244 if(m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f;
245 if(m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f;
246 if(m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f;
247 if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f;
248 if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f;
249 if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f;
250 m_angularMotorApply = 10;
251 break;
252 case Vehicle.LINEAR_FRICTION_TIMESCALE:
253 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
254 break;
255 case Vehicle.LINEAR_MOTOR_DIRECTION:
256 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
257 m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
258 break;
259 case Vehicle.LINEAR_MOTOR_OFFSET:
260 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
261 break;
262 }
263
264 }//end ProcessVectorVehicleParam
265
266 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
267 {
268 switch (pParam)
269 {
270 case Vehicle.REFERENCE_FRAME:
271 // m_referenceFrame = pValue;
272 break;
273 }
274
275 }//end ProcessRotationVehicleParam
276
277 internal void ProcessTypeChange(Vehicle pType)
278 {
279 // Set Defaults For Type
280 m_type = pType;
281 switch (pType)
282 {
283 case Vehicle.TYPE_SLED:
284 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
285 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
286 m_linearMotorDirection = Vector3.Zero;
287 m_linearMotorTimescale = 1000;
288 m_linearMotorDecayTimescale = 120;
289 m_angularMotorDirection = Vector3.Zero;
290 m_angularMotorTimescale = 1000;
291 m_angularMotorDecayTimescale = 120;
292 m_VhoverHeight = 0;
293 m_VhoverEfficiency = 1;
294 m_VhoverTimescale = 10;
295 m_VehicleBuoyancy = 0;
296 // m_linearDeflectionEfficiency = 1;
297 // m_linearDeflectionTimescale = 1;
298 // m_angularDeflectionEfficiency = 1;
299 // m_angularDeflectionTimescale = 1000;
300 // m_bankingEfficiency = 0;
301 // m_bankingMix = 1;
302 // m_bankingTimescale = 10;
303 // m_referenceFrame = Quaternion.Identity;
304 m_flags &=
305 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
306 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
307 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
308 break;
309 case Vehicle.TYPE_CAR:
310 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
311 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
312 m_linearMotorDirection = Vector3.Zero;
313 m_linearMotorTimescale = 1;
314 m_linearMotorDecayTimescale = 60;
315 m_angularMotorDirection = Vector3.Zero;
316 m_angularMotorTimescale = 1;
317 m_angularMotorDecayTimescale = 0.8f;
318 m_VhoverHeight = 0;
319 m_VhoverEfficiency = 0;
320 m_VhoverTimescale = 1000;
321 m_VehicleBuoyancy = 0;
322 // // m_linearDeflectionEfficiency = 1;
323 // // m_linearDeflectionTimescale = 2;
324 // // m_angularDeflectionEfficiency = 0;
325 // m_angularDeflectionTimescale = 10;
326 m_verticalAttractionEfficiency = 1f;
327 m_verticalAttractionTimescale = 10f;
328 // m_bankingEfficiency = -0.2f;
329 // m_bankingMix = 1;
330 // m_bankingTimescale = 1;
331 // m_referenceFrame = Quaternion.Identity;
332 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
333 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY |
334 VehicleFlag.LIMIT_MOTOR_UP);
335 break;
336 case Vehicle.TYPE_BOAT:
337 m_linearFrictionTimescale = new Vector3(10, 3, 2);
338 m_angularFrictionTimescale = new Vector3(10,10,10);
339 m_linearMotorDirection = Vector3.Zero;
340 m_linearMotorTimescale = 5;
341 m_linearMotorDecayTimescale = 60;
342 m_angularMotorDirection = Vector3.Zero;
343 m_angularMotorTimescale = 4;
344 m_angularMotorDecayTimescale = 4;
345 m_VhoverHeight = 0;
346 m_VhoverEfficiency = 0.5f;
347 m_VhoverTimescale = 2;
348 m_VehicleBuoyancy = 1;
349 // m_linearDeflectionEfficiency = 0.5f;
350 // m_linearDeflectionTimescale = 3;
351 // m_angularDeflectionEfficiency = 0.5f;
352 // m_angularDeflectionTimescale = 5;
353 m_verticalAttractionEfficiency = 0.5f;
354 m_verticalAttractionTimescale = 5f;
355 // m_bankingEfficiency = -0.3f;
356 // m_bankingMix = 0.8f;
357 // m_bankingTimescale = 1;
358 // m_referenceFrame = Quaternion.Identity;
359 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY |
360 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
361 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY |
362 VehicleFlag.LIMIT_MOTOR_UP);
363 break;
364 case Vehicle.TYPE_AIRPLANE:
365 m_linearFrictionTimescale = new Vector3(200, 10, 5);
366 m_angularFrictionTimescale = new Vector3(20, 20, 20);
367 m_linearMotorDirection = Vector3.Zero;
368 m_linearMotorTimescale = 2;
369 m_linearMotorDecayTimescale = 60;
370 m_angularMotorDirection = Vector3.Zero;
371 m_angularMotorTimescale = 4;
372 m_angularMotorDecayTimescale = 4;
373 m_VhoverHeight = 0;
374 m_VhoverEfficiency = 0.5f;
375 m_VhoverTimescale = 1000;
376 m_VehicleBuoyancy = 0;
377 // m_linearDeflectionEfficiency = 0.5f;
378 // m_linearDeflectionTimescale = 3;
379 // m_angularDeflectionEfficiency = 1;
380 // m_angularDeflectionTimescale = 2;
381 m_verticalAttractionEfficiency = 0.9f;
382 m_verticalAttractionTimescale = 2f;
383 // m_bankingEfficiency = 1;
384 // m_bankingMix = 0.7f;
385 // m_bankingTimescale = 2;
386 // m_referenceFrame = Quaternion.Identity;
387 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
388 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
389 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
390 break;
391 case Vehicle.TYPE_BALLOON:
392 m_linearFrictionTimescale = new Vector3(5, 5, 5);
393 m_angularFrictionTimescale = new Vector3(10, 10, 10);
394 m_linearMotorDirection = Vector3.Zero;
395 m_linearMotorTimescale = 5;
396 m_linearMotorDecayTimescale = 60;
397 m_angularMotorDirection = Vector3.Zero;
398 m_angularMotorTimescale = 6;
399 m_angularMotorDecayTimescale = 10;
400 m_VhoverHeight = 5;
401 m_VhoverEfficiency = 0.8f;
402 m_VhoverTimescale = 10;
403 m_VehicleBuoyancy = 1;
404 // m_linearDeflectionEfficiency = 0;
405 // m_linearDeflectionTimescale = 5;
406 // m_angularDeflectionEfficiency = 0;
407 // m_angularDeflectionTimescale = 5;
408 m_verticalAttractionEfficiency = 1f;
409 m_verticalAttractionTimescale = 100f;
410 // m_bankingEfficiency = 0;
411 // m_bankingMix = 0.7f;
412 // m_bankingTimescale = 5;
413 // m_referenceFrame = Quaternion.Identity;
414 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
415 VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
416 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
417 break;
418
419 }
420 }//end SetDefaultsForType
421
422 internal void Enable(IntPtr pBody, OdeScene pParentScene)
423 {
424 if (m_type == Vehicle.TYPE_NONE)
425 return;
426
427 m_body = pBody;
428 }
429
430 internal void Step(float pTimestep, OdeScene pParentScene)
431 {
432 if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
433 return;
434 frcount++; // used to limit debug comment output
435 if (frcount > 100)
436 frcount = 0;
437
438 MoveLinear(pTimestep, pParentScene);
439 MoveAngular(pTimestep);
440 }// end Step
441
442 private void MoveLinear(float pTimestep, OdeScene _pParentScene)
443 {
444 if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant
445 {
446 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
447
448 // add drive to body
449 Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep);
450 m_lastLinearVelocityVector += (addAmount*10); // lastLinearVelocityVector is the current body velocity vector?
451
452 // This will work temporarily, but we really need to compare speed on an axis
453 // KF: Limit body velocity to applied velocity?
454 if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X))
455 m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X;
456 if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y))
457 m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y;
458 if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z))
459 m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z;
460
461 // decay applied velocity
462 Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep)));
463 //Console.WriteLine("decay: " + decayfraction);
464 m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f;
465 //Console.WriteLine("actual: " + m_linearMotorDirection);
466 }
467 else
468 { // requested is not significant
469 // if what remains of applied is small, zero it.
470 if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
471 m_lastLinearVelocityVector = Vector3.Zero;
472 }
473
474
475 // convert requested object velocity to world-referenced vector
476 m_dir = m_lastLinearVelocityVector;
477 d.Quaternion rot = d.BodyGetQuaternion(Body);
478 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
479 m_dir *= rotq; // apply obj rotation to velocity vector
480
481 // add Gravity andBuoyancy
482 // KF: So far I have found no good method to combine a script-requested
483 // .Z velocity and gravity. Therefore only 0g will used script-requested
484 // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only.
485 Vector3 grav = Vector3.Zero;
486 if(m_VehicleBuoyancy < 1.0f)
487 {
488 // There is some gravity, make a gravity force vector
489 // that is applied after object velocity.
490 d.Mass objMass;
491 d.BodyGetMass(Body, out objMass);
492 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
493 grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy);
494 // Preserve the current Z velocity
495 d.Vector3 vel_now = d.BodyGetLinearVel(Body);
496 m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity
497 } // else its 1.0, no gravity.
498
499 // Check if hovering
500 if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
501 {
502 // We should hover, get the target height
503 d.Vector3 pos = d.BodyGetPosition(Body);
504 if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
505 {
506 m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight;
507 }
508 else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
509 {
510 m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
511 }
512 else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
513 {
514 m_VhoverTargetHeight = m_VhoverHeight;
515 }
516
517 if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
518 {
519 // If body is aready heigher, use its height as target height
520 if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
521 }
522
523// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
524// m_VhoverTimescale = 0f; // time to acheive height
525// pTimestep is time since last frame,in secs
526 float herr0 = pos.Z - m_VhoverTargetHeight;
527 // Replace Vertical speed with correction figure if significant
528 if(Math.Abs(herr0) > 0.01f )
529 {
530 d.Mass objMass;
531 d.BodyGetMass(Body, out objMass);
532 m_dir.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
533 //KF: m_VhoverEfficiency is not yet implemented
534 }
535 else
536 {
537 m_dir.Z = 0f;
538 }
539 }
540
541 // Apply velocity
542 d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z);
543 // apply gravity force
544 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
545
546
547 // apply friction
548 Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
549 m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
550 } // end MoveLinear()
551
552 private void MoveAngular(float pTimestep)
553 {
554 /*
555 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
556 private int m_angularMotorApply = 0; // application frame counter
557 private float m_angularMotorVelocity = 0; // current angular motor velocity (ramps up and down)
558 private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate
559 private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate
560 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
561 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
562 */
563
564 // Get what the body is doing, this includes 'external' influences
565 d.Vector3 angularVelocity = d.BodyGetAngularVel(Body);
566 // Vector3 angularVelocity = Vector3.Zero;
567
568 if (m_angularMotorApply > 0)
569 {
570 // ramp up to new value
571 // current velocity += error / ( time to get there / step interval )
572 // requested speed - last motor speed
573 m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep);
574 m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep);
575 m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep);
576
577 m_angularMotorApply--; // This is done so that if script request rate is less than phys frame rate the expected
578 // velocity may still be acheived.
579 }
580 else
581 {
582 // no motor recently applied, keep the body velocity
583 /* m_angularMotorVelocity.X = angularVelocity.X;
584 m_angularMotorVelocity.Y = angularVelocity.Y;
585 m_angularMotorVelocity.Z = angularVelocity.Z; */
586
587 // and decay the velocity
588 m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep);
589 } // end motor section
590
591
592 // Vertical attractor section
593 Vector3 vertattr = Vector3.Zero;
594
595 if(m_verticalAttractionTimescale < 300)
596 {
597 float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
598 // get present body rotation
599 d.Quaternion rot = d.BodyGetQuaternion(Body);
600 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
601 // make a vector pointing up
602 Vector3 verterr = Vector3.Zero;
603 verterr.Z = 1.0f;
604 // rotate it to Body Angle
605 verterr = verterr * rotq;
606 // 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.
607 // 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
608 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
609 if (verterr.Z < 0.0f)
610 {
611 verterr.X = 2.0f - verterr.X;
612 verterr.Y = 2.0f - verterr.Y;
613 }
614 // Error is 0 (no error) to +/- 2 (max error)
615 // scale it by VAservo
616 verterr = verterr * VAservo;
617//if(frcount == 0) Console.WriteLine("VAerr=" + verterr);
618
619 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
620 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
621 vertattr.X = verterr.Y;
622 vertattr.Y = - verterr.X;
623 vertattr.Z = 0f;
624
625 // scaling appears better usingsquare-law
626 float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
627 vertattr.X += bounce * angularVelocity.X;
628 vertattr.Y += bounce * angularVelocity.Y;
629
630 } // else vertical attractor is off
631
632 // m_lastVertAttractor = vertattr;
633
634 // Bank section tba
635 // Deflection section tba
636
637 // Sum velocities
638 m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection
639
640 if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
641 {
642 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
643 }
644 else
645 {
646 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
647 }
648
649 // apply friction
650 Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
651 m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;
652
653 // Apply to the body
654 d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
655
656 } //end MoveAngular
657 }
658}
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index 4581d22..864ea80 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -1,4 +1,15 @@
1/* 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.
2 * Copyright (c) Contributors, http://opensimulator.org/ 13 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders. 14 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 * 15 *
@@ -72,6 +83,9 @@ namespace OpenSim.Region.Physics.OdePlugin
72 private float PID_G = 25f; 83 private float PID_G = 25f;
73 private bool m_usePID = false; 84 private bool m_usePID = false;
74 85
86 // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
87 // and are for non-VEHICLES only.
88
75 private float m_PIDHoverHeight = 0f; 89 private float m_PIDHoverHeight = 0f;
76 private float m_PIDHoverTau = 0f; 90 private float m_PIDHoverTau = 0f;
77 private bool m_useHoverPID = false; 91 private bool m_useHoverPID = false;
@@ -79,6 +93,7 @@ namespace OpenSim.Region.Physics.OdePlugin
79 private float m_targetHoverHeight = 0f; 93 private float m_targetHoverHeight = 0f;
80 private float m_groundHeight = 0f; 94 private float m_groundHeight = 0f;
81 private float m_waterHeight = 0f; 95 private float m_waterHeight = 0f;
96 private float m_buoyancy = 0f; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
82 97
83 // private float m_tensor = 5f; 98 // private float m_tensor = 5f;
84 private int body_autodisable_frames = 20; 99 private int body_autodisable_frames = 20;
@@ -146,8 +161,6 @@ namespace OpenSim.Region.Physics.OdePlugin
146 public int m_roundsUnderMotionThreshold = 0; 161 public int m_roundsUnderMotionThreshold = 0;
147 private int m_crossingfailures = 0; 162 private int m_crossingfailures = 0;
148 163
149 public float m_buoyancy = 0f;
150
151 public bool outofBounds = false; 164 public bool outofBounds = false;
152 private float m_density = 10.000006836f; // Aluminum g/cm3; 165 private float m_density = 10.000006836f; // Aluminum g/cm3;
153 166
@@ -155,7 +168,8 @@ namespace OpenSim.Region.Physics.OdePlugin
155 private bool m_lastUpdateSent = false; 168 private bool m_lastUpdateSent = false;
156 169
157 public IntPtr Body = (IntPtr) 0; 170 public IntPtr Body = (IntPtr) 0;
158 private String m_primName; 171 public String m_primName;
172// private String m_primName;
159 private PhysicsVector _target_velocity; 173 private PhysicsVector _target_velocity;
160 public d.Mass pMass; 174 public d.Mass pMass;
161 175
@@ -166,7 +180,7 @@ namespace OpenSim.Region.Physics.OdePlugin
166 180
167 public volatile bool childPrim = false; 181 public volatile bool childPrim = false;
168 182
169 private ODEVehicleSettings m_vehicle; 183 private ODEDynamics m_vehicle;
170 184
171 internal int m_material = (int)Material.Wood; 185 internal int m_material = (int)Material.Wood;
172 186
@@ -174,7 +188,7 @@ namespace OpenSim.Region.Physics.OdePlugin
174 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode) 188 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode)
175 { 189 {
176 _target_velocity = new PhysicsVector(0, 0, 0); 190 _target_velocity = new PhysicsVector(0, 0, 0);
177 m_vehicle = new ODEVehicleSettings(); 191 m_vehicle = new ODEDynamics();
178 //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned); 192 //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
179 ode = dode; 193 ode = dode;
180 _velocity = new PhysicsVector(); 194 _velocity = new PhysicsVector();
@@ -267,6 +281,8 @@ namespace OpenSim.Region.Physics.OdePlugin
267 public override bool Selected 281 public override bool Selected
268 { 282 {
269 set { 283 set {
284
285
270 // This only makes the object not collidable if the object 286 // This only makes the object not collidable if the object
271 // is physical or the object is modified somehow *IN THE FUTURE* 287 // is physical or the object is modified somehow *IN THE FUTURE*
272 // without this, if an avatar selects prim, they can walk right 288 // without this, if an avatar selects prim, they can walk right
@@ -282,6 +298,7 @@ namespace OpenSim.Region.Physics.OdePlugin
282 m_taintselected = value; 298 m_taintselected = value;
283 m_isSelected = value; 299 m_isSelected = value;
284 } 300 }
301 if(m_isSelected) disableBodySoft();
285 } 302 }
286 } 303 }
287 304
@@ -289,6 +306,7 @@ namespace OpenSim.Region.Physics.OdePlugin
289 { 306 {
290 prev_geom = prim_geom; 307 prev_geom = prim_geom;
291 prim_geom = geom; 308 prim_geom = geom;
309//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName);
292 if (prim_geom != IntPtr.Zero) 310 if (prim_geom != IntPtr.Zero)
293 { 311 {
294 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 312 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
@@ -300,6 +318,7 @@ namespace OpenSim.Region.Physics.OdePlugin
300 if (_parent != null && _parent is OdePrim) 318 if (_parent != null && _parent is OdePrim)
301 { 319 {
302 OdePrim parent = (OdePrim)_parent; 320 OdePrim parent = (OdePrim)_parent;
321//Console.WriteLine("SetGeom calls ChildSetGeom");
303 parent.ChildSetGeom(this); 322 parent.ChildSetGeom(this);
304 } 323 }
305 } 324 }
@@ -315,7 +334,8 @@ namespace OpenSim.Region.Physics.OdePlugin
315 if (m_isphysical && Body != IntPtr.Zero) 334 if (m_isphysical && Body != IntPtr.Zero)
316 { 335 {
317 d.BodyEnable(Body); 336 d.BodyEnable(Body);
318 m_vehicle.Enable(Body, _parent_scene); 337 if (m_vehicle.Type != Vehicle.TYPE_NONE)
338 m_vehicle.Enable(Body, _parent_scene);
319 } 339 }
320 340
321 m_disabled = false; 341 m_disabled = false;
@@ -329,7 +349,6 @@ namespace OpenSim.Region.Physics.OdePlugin
329 if (m_isphysical && Body != IntPtr.Zero) 349 if (m_isphysical && Body != IntPtr.Zero)
330 { 350 {
331 d.BodyDisable(Body); 351 d.BodyDisable(Body);
332 m_vehicle.Disable();
333 } 352 }
334 } 353 }
335 354
@@ -359,6 +378,9 @@ namespace OpenSim.Region.Physics.OdePlugin
359 378
360 d.BodySetAutoDisableFlag(Body, true); 379 d.BodySetAutoDisableFlag(Body, true);
361 d.BodySetAutoDisableSteps(Body, body_autodisable_frames); 380 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
381
382 // disconnect from world gravity so we can apply buoyancy
383 d.BodySetGravityMode (Body, false);
362 384
363 m_interpenetrationcount = 0; 385 m_interpenetrationcount = 0;
364 m_collisionscore = 0; 386 m_collisionscore = 0;
@@ -705,13 +727,8 @@ namespace OpenSim.Region.Physics.OdePlugin
705 break; 727 break;
706 } 728 }
707 } 729 }
708
709
710
711
712
713 return returnMass; 730 return returnMass;
714 } 731 }// end CalculateMass
715 732
716 #endregion 733 #endregion
717 734
@@ -737,7 +754,6 @@ namespace OpenSim.Region.Physics.OdePlugin
737 if (Body != IntPtr.Zero) 754 if (Body != IntPtr.Zero)
738 { 755 {
739 _parent_scene.remActivePrim(this); 756 _parent_scene.remActivePrim(this);
740 m_vehicle.Destroy();
741 m_collisionCategories &= ~CollisionCategories.Body; 757 m_collisionCategories &= ~CollisionCategories.Body;
742 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); 758 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
743 759
@@ -827,6 +843,7 @@ namespace OpenSim.Region.Physics.OdePlugin
827 { 843 {
828 if (prim_geom == IntPtr.Zero) 844 if (prim_geom == IntPtr.Zero)
829 { 845 {
846//Console.WriteLine(" setMesh 1");
830 SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null)); 847 SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
831 } 848 }
832 } 849 }
@@ -848,19 +865,35 @@ namespace OpenSim.Region.Physics.OdePlugin
848 865
849 public void ProcessTaints(float timestep) 866 public void ProcessTaints(float timestep)
850 { 867 {
868//Console.WriteLine("ProcessTaints for " + m_primName );
851 if (m_taintadd) 869 if (m_taintadd)
852 { 870 {
853 changeadd(timestep); 871 changeadd(timestep);
854 } 872 }
873
855 if (prim_geom != IntPtr.Zero) 874 if (prim_geom != IntPtr.Zero)
856 { 875 {
857 if (!_position.IsIdentical(m_taintposition,0f)) 876 if (!_position.IsIdentical(m_taintposition,0f))
858 changemove(timestep); 877 changemove(timestep);
859 878
860 if (m_taintrot != _orientation) 879 if (m_taintrot != _orientation)
861 rotate(timestep); 880 {
881 if(childPrim && IsPhysical) // For physical child prim...
882 {
883 rotate(timestep);
884 // KF: ODE will also rotate the parent prim!
885 // so rotate the root back to where it was
886 OdePrim parent = (OdePrim)_parent;
887 parent.rotate(timestep);
888 }
889 else
890 {
891 //Just rotate the prim
892 rotate(timestep);
893 }
894 }
862 // 895 //
863 896
864 if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent)) 897 if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent))
865 changePhysicsStatus(timestep); 898 changePhysicsStatus(timestep);
866 // 899 //
@@ -899,8 +932,7 @@ namespace OpenSim.Region.Physics.OdePlugin
899 932
900 if (!m_angularlock.IsIdentical(m_taintAngularLock,0)) 933 if (!m_angularlock.IsIdentical(m_taintAngularLock,0))
901 changeAngularLock(timestep); 934 changeAngularLock(timestep);
902 935
903
904 } 936 }
905 else 937 else
906 { 938 {
@@ -932,11 +964,6 @@ namespace OpenSim.Region.Physics.OdePlugin
932 Amotor = IntPtr.Zero; 964 Amotor = IntPtr.Zero;
933 } 965 }
934 } 966 }
935
936 if (m_vehicle.Type != Vehicle.TYPE_NONE)
937 {
938 m_vehicle.Reset();
939 }
940 } 967 }
941 } 968 }
942 // Store this for later in case we get turned into a separate body 969 // Store this for later in case we get turned into a separate body
@@ -954,7 +981,7 @@ namespace OpenSim.Region.Physics.OdePlugin
954 { 981 {
955 OdePrim obj = (OdePrim)m_taintparent; 982 OdePrim obj = (OdePrim)m_taintparent;
956 //obj.disableBody(); 983 //obj.disableBody();
957 984//Console.WriteLine("changelink calls ParentPrim");
958 obj.ParentPrim(this); 985 obj.ParentPrim(this);
959 986
960 /* 987 /*
@@ -972,6 +999,8 @@ namespace OpenSim.Region.Physics.OdePlugin
972 // destroy link 999 // destroy link
973 else if (_parent != null && m_taintparent == null) 1000 else if (_parent != null && m_taintparent == null)
974 { 1001 {
1002//Console.WriteLine(" changelink B");
1003
975 if (_parent is OdePrim) 1004 if (_parent is OdePrim)
976 { 1005 {
977 OdePrim obj = (OdePrim)_parent; 1006 OdePrim obj = (OdePrim)_parent;
@@ -988,7 +1017,7 @@ namespace OpenSim.Region.Physics.OdePlugin
988 m_linkJoint = (IntPtr)0; 1017 m_linkJoint = (IntPtr)0;
989 */ 1018 */
990 } 1019 }
991 1020
992 _parent = m_taintparent; 1021 _parent = m_taintparent;
993 m_taintPhysics = m_isphysical; 1022 m_taintPhysics = m_isphysical;
994 } 1023 }
@@ -997,6 +1026,7 @@ namespace OpenSim.Region.Physics.OdePlugin
997 // prim is the child 1026 // prim is the child
998 public void ParentPrim(OdePrim prim) 1027 public void ParentPrim(OdePrim prim)
999 { 1028 {
1029//Console.WriteLine("ParentPrim " + m_primName);
1000 if (this.m_localID != prim.m_localID) 1030 if (this.m_localID != prim.m_localID)
1001 { 1031 {
1002 if (Body == IntPtr.Zero) 1032 if (Body == IntPtr.Zero)
@@ -1010,6 +1040,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1010 { 1040 {
1011 if (!childrenPrim.Contains(prim)) 1041 if (!childrenPrim.Contains(prim))
1012 { 1042 {
1043//Console.WriteLine("childrenPrim.Add " + prim);
1013 childrenPrim.Add(prim); 1044 childrenPrim.Add(prim);
1014 1045
1015 foreach (OdePrim prm in childrenPrim) 1046 foreach (OdePrim prm in childrenPrim)
@@ -1033,6 +1064,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1033 } 1064 }
1034 foreach (OdePrim prm in childrenPrim) 1065 foreach (OdePrim prm in childrenPrim)
1035 { 1066 {
1067
1036 prm.m_collisionCategories |= CollisionCategories.Body; 1068 prm.m_collisionCategories |= CollisionCategories.Body;
1037 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); 1069 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1038 1070
@@ -1041,7 +1073,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1041 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet"); 1073 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet");
1042 continue; 1074 continue;
1043 } 1075 }
1044 1076//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + m_primName);
1045 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); 1077 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
1046 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); 1078 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
1047 1079
@@ -1086,11 +1118,12 @@ namespace OpenSim.Region.Physics.OdePlugin
1086 prm.Body = Body; 1118 prm.Body = Body;
1087 _parent_scene.addActivePrim(prm); 1119 _parent_scene.addActivePrim(prm);
1088 } 1120 }
1089
1090 m_collisionCategories |= CollisionCategories.Body; 1121 m_collisionCategories |= CollisionCategories.Body;
1091 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); 1122 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1092 1123
1124//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + m_primName);
1093 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 1125 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1126//Console.WriteLine(" Post GeomSetCategoryBits 2");
1094 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 1127 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1095 1128
1096 1129
@@ -1126,7 +1159,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1126 createAMotor(m_angularlock); 1159 createAMotor(m_angularlock);
1127 } 1160 }
1128 d.BodySetPosition(Body, Position.X, Position.Y, Position.Z); 1161 d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
1129 m_vehicle.Enable(Body, _parent_scene); 1162 if (m_vehicle.Type != Vehicle.TYPE_NONE) m_vehicle.Enable(Body, _parent_scene);
1130 _parent_scene.addActivePrim(this); 1163 _parent_scene.addActivePrim(this);
1131 } 1164 }
1132 } 1165 }
@@ -1163,6 +1196,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1163 { 1196 {
1164 foreach (OdePrim prm in childrenPrim) 1197 foreach (OdePrim prm in childrenPrim)
1165 { 1198 {
1199//Console.WriteLine("ChildSetGeom calls ParentPrim");
1166 ParentPrim(prm); 1200 ParentPrim(prm);
1167 } 1201 }
1168 } 1202 }
@@ -1189,6 +1223,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1189 1223
1190 lock (childrenPrim) 1224 lock (childrenPrim)
1191 { 1225 {
1226 //Console.WriteLine("childrenPrim.Remove " + odePrim);
1192 childrenPrim.Remove(odePrim); 1227 childrenPrim.Remove(odePrim);
1193 } 1228 }
1194 1229
@@ -1206,6 +1241,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1206 { 1241 {
1207 foreach (OdePrim prm in childrenPrim) 1242 foreach (OdePrim prm in childrenPrim)
1208 { 1243 {
1244//Console.WriteLine("ChildDelink calls ParentPrim");
1209 ParentPrim(prm); 1245 ParentPrim(prm);
1210 } 1246 }
1211 } 1247 }
@@ -1290,7 +1326,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1290 1326
1291 resetCollisionAccounting(); 1327 resetCollisionAccounting();
1292 m_isSelected = m_taintselected; 1328 m_isSelected = m_taintselected;
1293 } 1329 }//end changeSelectedStatus
1294 1330
1295 public void ResetTaints() 1331 public void ResetTaints()
1296 { 1332 {
@@ -1307,6 +1343,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1307 1343
1308 public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh) 1344 public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh)
1309 { 1345 {
1346//Console.WriteLine("CreateGeom:");
1310 if (_mesh != null) 1347 if (_mesh != null)
1311 { 1348 {
1312 setMesh(_parent_scene, _mesh); 1349 setMesh(_parent_scene, _mesh);
@@ -1322,6 +1359,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1322 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1359 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1323 try 1360 try
1324 { 1361 {
1362//Console.WriteLine(" CreateGeom 1");
1325 SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2)); 1363 SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2));
1326 } 1364 }
1327 catch (AccessViolationException) 1365 catch (AccessViolationException)
@@ -1336,6 +1374,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1336 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1374 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1337 try 1375 try
1338 { 1376 {
1377//Console.WriteLine(" CreateGeom 2");
1339 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); 1378 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1340 } 1379 }
1341 catch (AccessViolationException) 1380 catch (AccessViolationException)
@@ -1351,6 +1390,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1351 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1390 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1352 try 1391 try
1353 { 1392 {
1393//Console.WriteLine(" CreateGeom 3");
1354 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); 1394 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1355 } 1395 }
1356 catch (AccessViolationException) 1396 catch (AccessViolationException)
@@ -1367,6 +1407,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1367 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1407 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1368 try 1408 try
1369 { 1409 {
1410//Console.WriteLine(" CreateGeom 4");
1370 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); 1411 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1371 } 1412 }
1372 catch (AccessViolationException) 1413 catch (AccessViolationException)
@@ -1403,6 +1444,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1403 1444
1404 lock (_parent_scene.OdeLock) 1445 lock (_parent_scene.OdeLock)
1405 { 1446 {
1447//Console.WriteLine("changeadd 1");
1406 CreateGeom(m_targetSpace, _mesh); 1448 CreateGeom(m_targetSpace, _mesh);
1407 1449
1408 if (prim_geom != IntPtr.Zero) 1450 if (prim_geom != IntPtr.Zero)
@@ -1458,6 +1500,8 @@ namespace OpenSim.Region.Physics.OdePlugin
1458 OdePrim odParent = (OdePrim)_parent; 1500 OdePrim odParent = (OdePrim)_parent;
1459 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body) 1501 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
1460 { 1502 {
1503// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
1504Console.WriteLine(" JointCreateFixed");
1461 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); 1505 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
1462 d.JointAttach(m_linkJoint, Body, odParent.Body); 1506 d.JointAttach(m_linkJoint, Body, odParent.Body);
1463 d.JointSetFixed(m_linkJoint); 1507 d.JointSetFixed(m_linkJoint);
@@ -1511,239 +1555,236 @@ namespace OpenSim.Region.Physics.OdePlugin
1511 float fz = 0; 1555 float fz = 0;
1512 1556
1513 1557
1514 if (IsPhysical && Body != IntPtr.Zero && !m_isSelected) 1558 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
1515 { 1559 {
1516 if (d.BodyIsEnabled(Body) && !m_angularlock.IsIdentical(PhysicsVector.Zero, 0.003f)) 1560 if (m_vehicle.Type != Vehicle.TYPE_NONE)
1517 { 1561 {
1518 d.Vector3 avel2 = d.BodyGetAngularVel(Body); 1562 // 'VEHICLES' are dealt with in ODEDynamics.cs
1519 if (m_angularlock.X == 1) 1563 m_vehicle.Step(timestep, _parent_scene);
1520 avel2.X = 0; 1564 }
1521 if (m_angularlock.Y == 1) 1565 else
1522 avel2.Y = 0; 1566 {
1523 if (m_angularlock.Z == 1) 1567 // NON-'VEHICLES' are dealt with here
1524 avel2.Z = 0; 1568 if (d.BodyIsEnabled(Body) && !m_angularlock.IsIdentical(PhysicsVector.Zero, 0.003f))
1525 d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z); 1569 {
1526 } 1570 d.Vector3 avel2 = d.BodyGetAngularVel(Body);
1527 //float PID_P = 900.0f; 1571 if (m_angularlock.X == 1)
1528 1572 avel2.X = 0;
1529 float m_mass = CalculateMass(); 1573 if (m_angularlock.Y == 1)
1530 1574 avel2.Y = 0;
1531 fz = 0f; 1575 if (m_angularlock.Z == 1)
1576 avel2.Z = 0;
1577 d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
1578 }
1579 //float PID_P = 900.0f;
1580
1581 float m_mass = CalculateMass();
1582
1583// fz = 0f;
1532 //m_log.Info(m_collisionFlags.ToString()); 1584 //m_log.Info(m_collisionFlags.ToString());
1533 1585
1534 if (m_buoyancy != 0) 1586
1535 { 1587 //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
1536 if (m_buoyancy > 0) 1588 // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ??
1537 { 1589 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
1538 fz = (((-1 * _parent_scene.gravityz) * m_buoyancy) * m_mass); 1590 // gravityz multiplier = 1 - m_buoyancy
1539 1591 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass;
1540 //d.Vector3 l_velocity = d.BodyGetLinearVel(Body); 1592
1541 //m_log.Info("Using Buoyancy: " + buoyancy + " G: " + (_parent_scene.gravityz * m_buoyancy) + "mass:" + m_mass + " Pos: " + Position.ToString()); 1593 if (m_usePID)
1542 } 1594 {
1543 else 1595 // KF - this is for object move? eg. llSetPos() ?
1544 { 1596 //if (!d.BodyIsEnabled(Body))
1545 fz = (-1 * (((-1 * _parent_scene.gravityz) * (-1 * m_buoyancy)) * m_mass)); 1597 //d.BodySetForce(Body, 0f, 0f, 0f);
1546 } 1598 // If we're using the PID controller, then we have no gravity
1547 } 1599 //fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply...
1548 1600 fz = 0f;
1549 if (m_usePID) 1601
1550 { 1602 // no lock; for now it's only called from within Simulate()
1551 1603
1552 //if (!d.BodyIsEnabled(Body)) 1604 // If the PID Controller isn't active then we set our force
1553 //d.BodySetForce(Body, 0f, 0f, 0f); 1605 // calculating base velocity to the current position
1554 // If we're using the PID controller, then we have no gravity 1606
1555 fz = (-1 * _parent_scene.gravityz) * m_mass; 1607 if ((m_PIDTau < 1) && (m_PIDTau != 0))
1556 1608 {
1557 // no lock; for now it's only called from within Simulate() 1609 //PID_G = PID_G / m_PIDTau;
1558 1610 m_PIDTau = 1;
1559 // If the PID Controller isn't active then we set our force 1611 }
1560 // calculating base velocity to the current position 1612
1561 1613 if ((PID_G - m_PIDTau) <= 0)
1562 if ((m_PIDTau < 1) && (m_PIDTau != 0)) 1614 {
1563 { 1615 PID_G = m_PIDTau + 1;
1564 //PID_G = PID_G / m_PIDTau; 1616 }
1565 m_PIDTau = 1; 1617 //PidStatus = true;
1566 } 1618
1567 1619 // PhysicsVector vec = new PhysicsVector();
1568 if ((PID_G - m_PIDTau) <= 0) 1620 d.Vector3 vel = d.BodyGetLinearVel(Body);
1569 { 1621
1570 PID_G = m_PIDTau + 1; 1622 d.Vector3 pos = d.BodyGetPosition(Body);
1571 } 1623 _target_velocity =
1572 //PidStatus = true; 1624 new PhysicsVector(
1573 1625 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
1574 // PhysicsVector vec = new PhysicsVector(); 1626 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
1575 d.Vector3 vel = d.BodyGetLinearVel(Body); 1627 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
1576 1628 );
1577 d.Vector3 pos = d.BodyGetPosition(Body); 1629
1578 _target_velocity = 1630 // if velocity is zero, use position control; otherwise, velocity control
1579 new PhysicsVector( 1631
1580 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), 1632 if (_target_velocity.IsIdentical(PhysicsVector.Zero,0.1f))
1581 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), 1633 {
1582 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) 1634 // keep track of where we stopped. No more slippin' & slidin'
1583 ); 1635
1584 1636 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1585 // if velocity is zero, use position control; otherwise, velocity control 1637 // react to the physics scene by moving it's position.
1586 1638 // Avatar to Avatar collisions
1587 if (_target_velocity.IsIdentical(PhysicsVector.Zero,0.1f)) 1639 // Prim to avatar collisions
1588 { 1640
1589 // keep track of where we stopped. No more slippin' & slidin' 1641 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
1590 1642 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
1591 // We only want to deactivate the PID Controller if we think we want to have our surrogate 1643 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
1592 // react to the physics scene by moving it's position. 1644 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
1593 // Avatar to Avatar collisions 1645 d.BodySetLinearVel(Body, 0, 0, 0);
1594 // Prim to avatar collisions 1646 d.BodyAddForce(Body, 0, 0, fz);
1595 1647 return;
1596 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); 1648 }
1597 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); 1649 else
1598 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; 1650 {
1599 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); 1651 _zeroFlag = false;
1600 d.BodySetLinearVel(Body, 0, 0, 0); 1652
1601 d.BodyAddForce(Body, 0, 0, fz); 1653 // We're flying and colliding with something
1602 return; 1654 fx = ((_target_velocity.X) - vel.X) * (PID_D);
1603 } 1655 fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
1604 else 1656
1605 { 1657 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
1606 _zeroFlag = false; 1658
1607 1659 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1608 // We're flying and colliding with something 1660 }
1609 fx = ((_target_velocity.X) - vel.X) * (PID_D); 1661 } // end if (m_usePID)
1610 fy = ((_target_velocity.Y) - vel.Y) * (PID_D); 1662
1611 1663 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
1612 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; 1664 if (m_useHoverPID && !m_usePID)
1613 1665 {
1614 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); 1666 // If we're using the PID controller, then we have no gravity
1615 } 1667 fz = (-1 * _parent_scene.gravityz) * m_mass;
1616 } 1668
1617 1669 // no lock; for now it's only called from within Simulate()
1618 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller 1670
1619 if (m_useHoverPID && !m_usePID) 1671 // If the PID Controller isn't active then we set our force
1620 { 1672 // calculating base velocity to the current position
1621 // If we're using the PID controller, then we have no gravity 1673
1622 fz = (-1 * _parent_scene.gravityz) * m_mass; 1674 if ((m_PIDTau < 1))
1623 1675 {
1624 // no lock; for now it's only called from within Simulate() 1676 PID_G = PID_G / m_PIDTau;
1625 1677 }
1626 // If the PID Controller isn't active then we set our force 1678
1627 // calculating base velocity to the current position 1679 if ((PID_G - m_PIDTau) <= 0)
1628 1680 {
1629 if ((m_PIDTau < 1)) 1681 PID_G = m_PIDTau + 1;
1630 { 1682 }
1631 PID_G = PID_G / m_PIDTau;
1632 }
1633
1634 if ((PID_G - m_PIDTau) <= 0)
1635 {
1636 PID_G = m_PIDTau + 1;
1637 }
1638 1683
1639 1684
1640 // Where are we, and where are we headed? 1685 // Where are we, and where are we headed?
1641 d.Vector3 pos = d.BodyGetPosition(Body); 1686 d.Vector3 pos = d.BodyGetPosition(Body);
1642 d.Vector3 vel = d.BodyGetLinearVel(Body); 1687 d.Vector3 vel = d.BodyGetLinearVel(Body);
1643 1688
1644 // determine what our target height really is based on HoverType 1689
1645 switch (m_PIDHoverType) 1690 // Non-Vehicles have a limited set of Hover options.
1646 { 1691 // determine what our target height really is based on HoverType
1647 case PIDHoverType.Absolute: 1692 switch (m_PIDHoverType)
1648 m_targetHoverHeight = m_PIDHoverHeight; 1693 {
1649 break; 1694 case PIDHoverType.Ground:
1650 case PIDHoverType.Ground: 1695 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1651 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); 1696 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1652 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; 1697 break;
1653 break; 1698 case PIDHoverType.GroundAndWater:
1654 case PIDHoverType.GroundAndWater: 1699 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1655 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); 1700 m_waterHeight = _parent_scene.GetWaterLevel();
1656 m_waterHeight = _parent_scene.GetWaterLevel(); 1701 if (m_groundHeight > m_waterHeight)
1657 if (m_groundHeight > m_waterHeight) 1702 {
1658 { 1703 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1659 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; 1704 }
1660 } 1705 else
1661 else 1706 {
1662 { 1707 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
1663 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; 1708 }
1664 } 1709 break;
1665 break; 1710
1666 case PIDHoverType.Water: 1711 } // end switch (m_PIDHoverType)
1667 m_waterHeight = _parent_scene.GetWaterLevel(); 1712
1668 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; 1713
1669 break; 1714 _target_velocity =
1670 } 1715 new PhysicsVector(0.0f, 0.0f,
1671 1716 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
1672 1717 );
1673 _target_velocity = 1718
1674 new PhysicsVector(0.0f, 0.0f, 1719 // if velocity is zero, use position control; otherwise, velocity control
1675 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) 1720
1676 ); 1721 if (_target_velocity.IsIdentical(PhysicsVector.Zero, 0.1f))
1677 1722 {
1678 // if velocity is zero, use position control; otherwise, velocity control 1723 // keep track of where we stopped. No more slippin' & slidin'
1679 1724
1680 if (_target_velocity.IsIdentical(PhysicsVector.Zero, 0.1f)) 1725 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1681 { 1726 // react to the physics scene by moving it's position.
1682 // keep track of where we stopped. No more slippin' & slidin' 1727 // Avatar to Avatar collisions
1683 1728 // Prim to avatar collisions
1684 // We only want to deactivate the PID Controller if we think we want to have our surrogate 1729
1685 // react to the physics scene by moving it's position. 1730 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
1686 // Avatar to Avatar collisions 1731 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
1687 // Prim to avatar collisions 1732 d.BodyAddForce(Body, 0, 0, fz);
1688 1733 return;
1689 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); 1734 }
1690 d.BodySetLinearVel(Body, vel.X, vel.Y, 0); 1735 else
1691 d.BodyAddForce(Body, 0, 0, fz); 1736 {
1692 return; 1737 _zeroFlag = false;
1693 } 1738
1694 else 1739 // We're flying and colliding with something
1695 { 1740 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1696 _zeroFlag = false; 1741 }
1697 1742 }
1698 // We're flying and colliding with something 1743
1699 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); 1744 fx *= m_mass;
1700 } 1745 fy *= m_mass;
1701 } 1746 //fz *= m_mass;
1702 1747
1703 fx *= m_mass; 1748 fx += m_force.X;
1704 fy *= m_mass; 1749 fy += m_force.Y;
1705 //fz *= m_mass; 1750 fz += m_force.Z;
1706 1751
1707 fx += m_force.X; 1752 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
1708 fy += m_force.Y; 1753 if (fx != 0 || fy != 0 || fz != 0)
1709 fz += m_force.Z; 1754 {
1710 1755 //m_taintdisable = true;
1711 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); 1756 //base.RaiseOutOfBounds(Position);
1712 if (fx != 0 || fy != 0 || fz != 0) 1757 //d.BodySetLinearVel(Body, fx, fy, 0f);
1713 { 1758 if (!d.BodyIsEnabled(Body))
1714 //m_taintdisable = true; 1759 {
1715 //base.RaiseOutOfBounds(Position); 1760 // A physical body at rest on a surface will auto-disable after a while,
1716 //d.BodySetLinearVel(Body, fx, fy, 0f); 1761 // this appears to re-enable it incase the surface it is upon vanishes,
1717 if (!d.BodyIsEnabled(Body)) 1762 // and the body should fall again.
1718 { 1763 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1719 d.BodySetLinearVel(Body, 0f, 0f, 0f); 1764 d.BodySetForce(Body, 0, 0, 0);
1720 d.BodySetForce(Body, 0, 0, 0); 1765 enableBodySoft();
1721 enableBodySoft(); 1766 }
1722 } 1767
1723 1768 // 35x10 = 350n times the mass per second applied maximum.
1724 // 35x10 = 350n times the mass per second applied maximum. 1769 float nmax = 35f * m_mass;
1725 float nmax = 35f * m_mass; 1770 float nmin = -35f * m_mass;
1726 float nmin = -35f * m_mass;
1727 1771
1728 1772
1729 if (fx > nmax) 1773 if (fx > nmax)
1730 fx = nmax; 1774 fx = nmax;
1731 if (fx < nmin) 1775 if (fx < nmin)
1732 fx = nmin; 1776 fx = nmin;
1733 if (fy > nmax) 1777 if (fy > nmax)
1734 fy = nmax; 1778 fy = nmax;
1735 if (fy < nmin) 1779 if (fy < nmin)
1736 fy = nmin; 1780 fy = nmin;
1737 d.BodyAddForce(Body, fx, fy, fz); 1781 d.BodyAddForce(Body, fx, fy, fz);
1738 } 1782 }
1739 if (m_vehicle.Body == IntPtr.Zero && m_vehicle.Type != Vehicle.TYPE_NONE) 1783 }
1740 m_vehicle.Enable(Body, _parent_scene);
1741
1742 m_vehicle.Step(timestep);
1743 } 1784 }
1744 else 1785 else
1745 { 1786 { // is not physical, or is not a body or is selected
1746 // _zeroPosition = d.BodyGetPosition(Body); 1787 // _zeroPosition = d.BodyGetPosition(Body);
1747 return; 1788 return;
1748 } 1789 }
1749 } 1790 }
@@ -1757,14 +1798,22 @@ namespace OpenSim.Region.Physics.OdePlugin
1757 myrot.Y = _orientation.Y; 1798 myrot.Y = _orientation.Y;
1758 myrot.Z = _orientation.Z; 1799 myrot.Z = _orientation.Z;
1759 myrot.W = _orientation.W; 1800 myrot.W = _orientation.W;
1760 d.GeomSetQuaternion(prim_geom, ref myrot); 1801 if (Body != IntPtr.Zero)
1761 if (m_isphysical && Body != IntPtr.Zero)
1762 { 1802 {
1803 // KF: If this is a root prim do BodySet
1763 d.BodySetQuaternion(Body, ref myrot); 1804 d.BodySetQuaternion(Body, ref myrot);
1764 if (!m_angularlock.IsIdentical(new PhysicsVector(1, 1, 1), 0)) 1805 if (m_isphysical)
1765 createAMotor(m_angularlock); 1806 {
1807 if (!m_angularlock.IsIdentical(new PhysicsVector(1, 1, 1), 0))
1808 createAMotor(m_angularlock);
1809 }
1810 }
1811 else
1812 {
1813 // daughter prim, do Geom set
1814 d.GeomSetQuaternion(prim_geom, ref myrot);
1766 } 1815 }
1767 1816
1768 resetCollisionAccounting(); 1817 resetCollisionAccounting();
1769 m_taintrot = _orientation; 1818 m_taintrot = _orientation;
1770 } 1819 }
@@ -1826,7 +1875,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1826 m_log.Error("[PHYSICS]: PrimGeom dead"); 1875 m_log.Error("[PHYSICS]: PrimGeom dead");
1827 } 1876 }
1828 } 1877 }
1829 1878//Console.WriteLine("changePhysicsStatus for " + m_primName );
1830 changeadd(2f); 1879 changeadd(2f);
1831 } 1880 }
1832 if (childPrim) 1881 if (childPrim)
@@ -1904,7 +1953,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1904 mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); 1953 mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
1905 1954
1906 //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); 1955 //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
1907 1956//Console.WriteLine("changesize 1");
1908 CreateGeom(m_targetSpace, mesh); 1957 CreateGeom(m_targetSpace, mesh);
1909 1958
1910 1959
@@ -1912,6 +1961,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1912 else 1961 else
1913 { 1962 {
1914 _mesh = null; 1963 _mesh = null;
1964//Console.WriteLine("changesize 2");
1915 CreateGeom(m_targetSpace, _mesh); 1965 CreateGeom(m_targetSpace, _mesh);
1916 } 1966 }
1917 1967
@@ -2018,6 +2068,7 @@ namespace OpenSim.Region.Physics.OdePlugin
2018 else 2068 else
2019 { 2069 {
2020 _mesh = null; 2070 _mesh = null;
2071//Console.WriteLine("changeshape");
2021 CreateGeom(m_targetSpace, null); 2072 CreateGeom(m_targetSpace, null);
2022 } 2073 }
2023 2074
@@ -2359,7 +2410,9 @@ namespace OpenSim.Region.Physics.OdePlugin
2359 set 2410 set
2360 { 2411 {
2361 if (QuaternionIsFinite(value)) 2412 if (QuaternionIsFinite(value))
2413 {
2362 _orientation = value; 2414 _orientation = value;
2415 }
2363 else 2416 else
2364 m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object"); 2417 m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object");
2365 2418
@@ -2578,12 +2631,16 @@ namespace OpenSim.Region.Physics.OdePlugin
2578 //outofBounds = true; 2631 //outofBounds = true;
2579 } 2632 }
2580 2633
2634 float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation));
2635//Console.WriteLine("Adiff " + m_primName + " = " + Adiff);
2581 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) 2636 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
2582 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) 2637 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
2583 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) 2638 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
2584 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01)) 2639// && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01))
2640 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large
2585 { 2641 {
2586 _zeroFlag = true; 2642 _zeroFlag = true;
2643//Console.WriteLine("ZFT 2");
2587 m_throttleUpdates = false; 2644 m_throttleUpdates = false;
2588 } 2645 }
2589 else 2646 else
diff --git a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
index ba289f7..0a065be 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
@@ -238,7 +238,8 @@ namespace OpenSim.Region.Physics.OdePlugin
238 private readonly HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>(); 238 private readonly HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
239 private readonly HashSet<OdePrim> _prims = new HashSet<OdePrim>(); 239 private readonly HashSet<OdePrim> _prims = new HashSet<OdePrim>();
240 private readonly HashSet<OdePrim> _activeprims = new HashSet<OdePrim>(); 240 private readonly HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
241 private readonly HashSet<OdePrim> _taintedPrim = new HashSet<OdePrim>(); 241 private readonly HashSet<OdePrim> _taintedPrimH = new HashSet<OdePrim>();
242 private readonly List<OdePrim> _taintedPrimL = new List<OdePrim>();
242 private readonly HashSet<OdeCharacter> _taintedActors = new HashSet<OdeCharacter>(); 243 private readonly HashSet<OdeCharacter> _taintedActors = new HashSet<OdeCharacter>();
243 private readonly List<d.ContactGeom> _perloopContact = new List<d.ContactGeom>(); 244 private readonly List<d.ContactGeom> _perloopContact = new List<d.ContactGeom>();
244 private readonly List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>(); 245 private readonly List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>();
@@ -2123,6 +2124,7 @@ namespace OpenSim.Region.Physics.OdePlugin
2123 /// <param name="prim"></param> 2124 /// <param name="prim"></param>
2124 public void RemovePrimThreadLocked(OdePrim prim) 2125 public void RemovePrimThreadLocked(OdePrim prim)
2125 { 2126 {
2127//Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName);
2126 lock (prim) 2128 lock (prim)
2127 { 2129 {
2128 remCollisionEventReporting(prim); 2130 remCollisionEventReporting(prim);
@@ -2570,11 +2572,15 @@ namespace OpenSim.Region.Physics.OdePlugin
2570 if (prim is OdePrim) 2572 if (prim is OdePrim)
2571 { 2573 {
2572 OdePrim taintedprim = ((OdePrim) prim); 2574 OdePrim taintedprim = ((OdePrim) prim);
2573 lock (_taintedPrim) 2575 lock (_taintedPrimH)
2574 { 2576 {
2575 if (!(_taintedPrim.Contains(taintedprim))) 2577 if (!(_taintedPrimH.Contains(taintedprim)))
2576 _taintedPrim.Add(taintedprim); 2578 {
2577 } 2579//Console.WriteLine("AddPhysicsActorTaint to " + taintedprim.m_primName);
2580 _taintedPrimH.Add(taintedprim); // HashSet for searching
2581 _taintedPrimL.Add(taintedprim); // List for ordered readout
2582 }
2583 }
2578 return; 2584 return;
2579 } 2585 }
2580 else if (prim is OdeCharacter) 2586 else if (prim is OdeCharacter)
@@ -2614,7 +2620,7 @@ namespace OpenSim.Region.Physics.OdePlugin
2614 float fps = 0; 2620 float fps = 0;
2615 //m_log.Info(timeStep.ToString()); 2621 //m_log.Info(timeStep.ToString());
2616 step_time += timeStep; 2622 step_time += timeStep;
2617 2623
2618 // If We're loaded down by something else, 2624 // If We're loaded down by something else,
2619 // or debugging with the Visual Studio project on pause 2625 // or debugging with the Visual Studio project on pause
2620 // skip a few frames to catch up gracefully. 2626 // skip a few frames to catch up gracefully.
@@ -2694,16 +2700,20 @@ namespace OpenSim.Region.Physics.OdePlugin
2694 // Modify other objects in the scene. 2700 // Modify other objects in the scene.
2695 processedtaints = false; 2701 processedtaints = false;
2696 2702
2697 lock (_taintedPrim) 2703 lock (_taintedPrimL)
2698 { 2704 {
2699 foreach (OdePrim prim in _taintedPrim) 2705 foreach (OdePrim prim in _taintedPrimL)
2700 { 2706 {
2707
2708
2701 if (prim.m_taintremove) 2709 if (prim.m_taintremove)
2702 { 2710 {
2711//Console.WriteLine("Simulate calls RemovePrimThreadLocked");
2703 RemovePrimThreadLocked(prim); 2712 RemovePrimThreadLocked(prim);
2704 } 2713 }
2705 else 2714 else
2706 { 2715 {
2716//Console.WriteLine("Simulate calls ProcessTaints");
2707 prim.ProcessTaints(timeStep); 2717 prim.ProcessTaints(timeStep);
2708 } 2718 }
2709 processedtaints = true; 2719 processedtaints = true;
@@ -2893,7 +2903,9 @@ namespace OpenSim.Region.Physics.OdePlugin
2893 } 2903 }
2894 2904
2895 if (processedtaints) 2905 if (processedtaints)
2896 _taintedPrim.Clear(); 2906//Console.WriteLine("Simulate calls Clear of _taintedPrim list");
2907 _taintedPrimH.Clear();
2908 _taintedPrimL.Clear();
2897 } 2909 }
2898 2910
2899 // Move characters 2911 // Move characters
@@ -3508,7 +3520,7 @@ namespace OpenSim.Region.Physics.OdePlugin
3508 public override void UnCombine(PhysicsScene pScene) 3520 public override void UnCombine(PhysicsScene pScene)
3509 { 3521 {
3510 IntPtr localGround = IntPtr.Zero; 3522 IntPtr localGround = IntPtr.Zero;
3511 //float[] localHeightfield; 3523 float[] localHeightfield;
3512 bool proceed = false; 3524 bool proceed = false;
3513 List<IntPtr> geomDestroyList = new List<IntPtr>(); 3525 List<IntPtr> geomDestroyList = new List<IntPtr>();
3514 3526
@@ -3520,7 +3532,7 @@ namespace OpenSim.Region.Physics.OdePlugin
3520 { 3532 {
3521 if (geom == localGround) 3533 if (geom == localGround)
3522 { 3534 {
3523 //localHeightfield = TerrainHeightFieldHeights[geom]; 3535 localHeightfield = TerrainHeightFieldHeights[geom];
3524 proceed = true; 3536 proceed = true;
3525 } 3537 }
3526 else 3538 else
@@ -3542,7 +3554,7 @@ namespace OpenSim.Region.Physics.OdePlugin
3542 // memory corruption 3554 // memory corruption
3543 if (TerrainHeightFieldHeights.ContainsKey(g)) 3555 if (TerrainHeightFieldHeights.ContainsKey(g))
3544 { 3556 {
3545 //float[] removingHeightField = TerrainHeightFieldHeights[g]; 3557 float[] removingHeightField = TerrainHeightFieldHeights[g];
3546 TerrainHeightFieldHeights.Remove(g); 3558 TerrainHeightFieldHeights.Remove(g);
3547 3559
3548 if (RegionTerrain.ContainsKey(g)) 3560 if (RegionTerrain.ContainsKey(g))
@@ -3554,10 +3566,12 @@ namespace OpenSim.Region.Physics.OdePlugin
3554 //removingHeightField = new float[0]; 3566 //removingHeightField = new float[0];
3555 } 3567 }
3556 } 3568 }
3569
3557 } 3570 }
3558 else 3571 else
3559 { 3572 {
3560 m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data."); 3573 m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data.");
3574
3561 } 3575 }
3562 } 3576 }
3563 } 3577 }
generated by cgit v1.1 at 2024-11-09 15:03:07 +0000