aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/OdePlugin
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs440
-rw-r--r--OpenSim/Region/Physics/OdePlugin/ODEPrim.cs562
-rw-r--r--OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs2
-rw-r--r--OpenSim/Region/Physics/OdePlugin/OdePlugin.cs20
4 files changed, 509 insertions, 515 deletions
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs
index 39cdc0f..008070b 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs
@@ -32,11 +32,11 @@
32 * ODEDynamics.cs contains methods dealing with Prim Physical motion 32 * ODEDynamics.cs contains methods dealing with Prim Physical motion
33 * (dynamics) and the associated settings. Old Linear and angular 33 * (dynamics) and the associated settings. Old Linear and angular
34 * motors for dynamic motion have been replace with MoveLinear() 34 * motors for dynamic motion have been replace with MoveLinear()
35 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic 35 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
36 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to 36 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
37 * switch between 'VEHICLE' parameter use and general dynamics 37 * switch between 'VEHICLE' parameter use and general dynamics
38 * settings use. 38 * settings use.
39 */ 39 */
40 40
41using System; 41using System;
42using System.Collections.Generic; 42using System.Collections.Generic;
@@ -53,7 +53,7 @@ namespace OpenSim.Region.Physics.OdePlugin
53 public class ODEDynamics 53 public class ODEDynamics
54 { 54 {
55 public Vehicle Type 55 public Vehicle Type
56 { 56 {
57 get { return m_type; } 57 get { return m_type; }
58 } 58 }
59 59
@@ -62,14 +62,14 @@ namespace OpenSim.Region.Physics.OdePlugin
62 get { return m_body; } 62 get { return m_body; }
63 } 63 }
64 64
65 private int frcount = 0; // Used to limit dynamics debug output to 65 private int frcount = 0; // Used to limit dynamics debug output to
66 // every 100th frame 66 // every 100th frame
67 67
68 // private OdeScene m_parentScene = null; 68 // private OdeScene m_parentScene = null;
69 private IntPtr m_body = IntPtr.Zero; 69 private IntPtr m_body = IntPtr.Zero;
70// private IntPtr m_jointGroup = IntPtr.Zero; 70// private IntPtr m_jointGroup = IntPtr.Zero;
71// private IntPtr m_aMotor = IntPtr.Zero; 71// private IntPtr m_aMotor = IntPtr.Zero;
72 72
73 73
74 // Vehicle properties 74 // Vehicle properties
75 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind 75 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
@@ -82,7 +82,7 @@ namespace OpenSim.Region.Physics.OdePlugin
82 // HOVER_UP_ONLY 82 // HOVER_UP_ONLY
83 // LIMIT_MOTOR_UP 83 // LIMIT_MOTOR_UP
84 // LIMIT_ROLL_ONLY 84 // LIMIT_ROLL_ONLY
85 85
86 // Linear properties 86 // Linear properties
87 private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time 87 private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time
88 private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL 88 private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL
@@ -91,47 +91,43 @@ namespace OpenSim.Region.Physics.OdePlugin
91 private float m_linearMotorDecayTimescale = 0; 91 private float m_linearMotorDecayTimescale = 0;
92 private float m_linearMotorTimescale = 0; 92 private float m_linearMotorTimescale = 0;
93 private Vector3 m_lastLinearVelocityVector = Vector3.Zero; 93 private Vector3 m_lastLinearVelocityVector = Vector3.Zero;
94 // private bool m_LinearMotorSetLastFrame = false; 94 // private bool m_LinearMotorSetLastFrame = false;
95 // private Vector3 m_linearMotorOffset = Vector3.Zero; 95 // private Vector3 m_linearMotorOffset = Vector3.Zero;
96 96
97 //Angular properties 97 //Angular properties
98 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor 98 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
99 private int m_angularMotorApply = 0; // application frame counter 99 private int m_angularMotorApply = 0; // application frame counter
100 private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity 100 private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity
101 private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate 101 private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate
102 private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate 102 private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate
103 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate 103 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
104 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body 104 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
105 // private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body 105 // private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body
106 106
107 //Deflection properties 107 //Deflection properties
108 // private float m_angularDeflectionEfficiency = 0; 108 // private float m_angularDeflectionEfficiency = 0;
109 // private float m_angularDeflectionTimescale = 0; 109 // private float m_angularDeflectionTimescale = 0;
110 // private float m_linearDeflectionEfficiency = 0; 110 // private float m_linearDeflectionEfficiency = 0;
111 // private float m_linearDeflectionTimescale = 0; 111 // private float m_linearDeflectionTimescale = 0;
112 112
113 //Banking properties 113 //Banking properties
114 // private float m_bankingEfficiency = 0; 114 // private float m_bankingEfficiency = 0;
115 // private float m_bankingMix = 0; 115 // private float m_bankingMix = 0;
116 // private float m_bankingTimescale = 0; 116 // private float m_bankingTimescale = 0;
117 117
118 //Hover and Buoyancy properties 118 //Hover and Buoyancy properties
119 private float m_VhoverHeight = 0f; 119 private float m_VhoverHeight = 0f;
120// private float m_VhoverEfficiency = 0f; 120// private float m_VhoverEfficiency = 0f;
121 private float m_VhoverTimescale = 0f; 121 private float m_VhoverTimescale = 0f;
122 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height 122 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
123 private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle. 123 private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle.
124 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) 124 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
125 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. 125 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
126 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. 126 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
127 127
128 //Attractor properties 128 //Attractor properties
129 private float m_verticalAttractionEfficiency = 1.0f; // damped 129 private float m_verticalAttractionEfficiency = 1.0f; // damped
130 private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor. 130 private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor.
131
132
133
134
135 131
136 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) 132 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
137 { 133 {
@@ -166,13 +162,13 @@ namespace OpenSim.Region.Physics.OdePlugin
166 // m_bankingTimescale = pValue; 162 // m_bankingTimescale = pValue;
167 break; 163 break;
168 case Vehicle.BUOYANCY: 164 case Vehicle.BUOYANCY:
169 if (pValue < -1f) pValue = -1f; 165 if (pValue < -1f) pValue = -1f;
170 if (pValue > 1f) pValue = 1f; 166 if (pValue > 1f) pValue = 1f;
171 m_VehicleBuoyancy = pValue; 167 m_VehicleBuoyancy = pValue;
172 break; 168 break;
173// case Vehicle.HOVER_EFFICIENCY: 169// case Vehicle.HOVER_EFFICIENCY:
174// if (pValue < 0f) pValue = 0f; 170// if (pValue < 0f) pValue = 0f;
175// if (pValue > 1f) pValue = 1f; 171// if (pValue > 1f) pValue = 1f;
176// m_VhoverEfficiency = pValue; 172// m_VhoverEfficiency = pValue;
177// break; 173// break;
178 case Vehicle.HOVER_HEIGHT: 174 case Vehicle.HOVER_HEIGHT:
@@ -199,7 +195,7 @@ namespace OpenSim.Region.Physics.OdePlugin
199 m_linearMotorTimescale = pValue; 195 m_linearMotorTimescale = pValue;
200 break; 196 break;
201 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: 197 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
202 if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable 198 if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable
203 if (pValue > 1.0f) pValue = 1.0f; 199 if (pValue > 1.0f) pValue = 1.0f;
204 m_verticalAttractionEfficiency = pValue; 200 m_verticalAttractionEfficiency = pValue;
205 break; 201 break;
@@ -207,8 +203,8 @@ namespace OpenSim.Region.Physics.OdePlugin
207 if (pValue < 0.01f) pValue = 0.01f; 203 if (pValue < 0.01f) pValue = 0.01f;
208 m_verticalAttractionTimescale = pValue; 204 m_verticalAttractionTimescale = pValue;
209 break; 205 break;
210 206
211 // These are vector properties but the engine lets you use a single float value to 207 // These are vector properties but the engine lets you use a single float value to
212 // set all of the components to the same value 208 // set all of the components to the same value
213 case Vehicle.ANGULAR_FRICTION_TIMESCALE: 209 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
214 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); 210 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
@@ -229,7 +225,6 @@ namespace OpenSim.Region.Physics.OdePlugin
229 break; 225 break;
230 226
231 } 227 }
232
233 }//end ProcessFloatVehicleParam 228 }//end ProcessFloatVehicleParam
234 229
235 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) 230 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
@@ -242,12 +237,12 @@ namespace OpenSim.Region.Physics.OdePlugin
242 case Vehicle.ANGULAR_MOTOR_DIRECTION: 237 case Vehicle.ANGULAR_MOTOR_DIRECTION:
243 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); 238 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
244 // Limit requested angular speed to 2 rps= 4 pi rads/sec 239 // Limit requested angular speed to 2 rps= 4 pi rads/sec
245 if(m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f; 240 if (m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f;
246 if(m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f; 241 if (m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f;
247 if(m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f; 242 if (m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f;
248 if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f; 243 if (m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f;
249 if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f; 244 if (m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f;
250 if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f; 245 if (m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f;
251 m_angularMotorApply = 10; 246 m_angularMotorApply = 10;
252 break; 247 break;
253 case Vehicle.LINEAR_FRICTION_TIMESCALE: 248 case Vehicle.LINEAR_FRICTION_TIMESCALE:
@@ -261,7 +256,6 @@ namespace OpenSim.Region.Physics.OdePlugin
261 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); 256 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
262 break; 257 break;
263 } 258 }
264
265 }//end ProcessVectorVehicleParam 259 }//end ProcessVectorVehicleParam
266 260
267 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) 261 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
@@ -272,12 +266,11 @@ namespace OpenSim.Region.Physics.OdePlugin
272 // m_referenceFrame = pValue; 266 // m_referenceFrame = pValue;
273 break; 267 break;
274 } 268 }
275
276 }//end ProcessRotationVehicleParam 269 }//end ProcessRotationVehicleParam
277 270
278 internal void ProcessTypeChange(Vehicle pType) 271 internal void ProcessTypeChange(Vehicle pType)
279 { 272 {
280 // Set Defaults For Type 273 // Set Defaults For Type
281 m_type = pType; 274 m_type = pType;
282 switch (pType) 275 switch (pType)
283 { 276 {
@@ -357,8 +350,8 @@ namespace OpenSim.Region.Physics.OdePlugin
357 // m_bankingMix = 0.8f; 350 // m_bankingMix = 0.8f;
358 // m_bankingTimescale = 1; 351 // m_bankingTimescale = 1;
359 // m_referenceFrame = Quaternion.Identity; 352 // m_referenceFrame = Quaternion.Identity;
360 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY | 353 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY |
361 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); 354 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
362 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | 355 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY |
363 VehicleFlag.LIMIT_MOTOR_UP); 356 VehicleFlag.LIMIT_MOTOR_UP);
364 break; 357 break;
@@ -432,24 +425,25 @@ namespace OpenSim.Region.Physics.OdePlugin
432 { 425 {
433 if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE) 426 if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
434 return; 427 return;
435 frcount++; // used to limit debug comment output 428 frcount++; // used to limit debug comment output
436 if (frcount > 100) 429 if (frcount > 100)
437 frcount = 0; 430 frcount = 0;
438 431
439 MoveLinear(pTimestep, pParentScene); 432 MoveLinear(pTimestep, pParentScene);
440 MoveAngular(pTimestep); 433 MoveAngular(pTimestep);
441 }// end Step 434 }// end Step
442 435
443 private void MoveLinear(float pTimestep, OdeScene _pParentScene) 436 private void MoveLinear(float pTimestep, OdeScene _pParentScene)
444 { 437 {
445 if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant 438 if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant
446 { 439 {
447 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); 440 if (!d.BodyIsEnabled(Body))
441 d.BodyEnable(Body);
448 442
449 // add drive to body 443 // add drive to body
450 Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep); 444 Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep);
451 m_lastLinearVelocityVector += (addAmount*10); // lastLinearVelocityVector is the current body velocity vector? 445 m_lastLinearVelocityVector += (addAmount*10); // lastLinearVelocityVector is the current body velocity vector?
452 446
453 // 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
454 // KF: Limit body velocity to applied velocity? 448 // KF: Limit body velocity to applied velocity?
455 if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X)) 449 if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X))
@@ -458,7 +452,7 @@ namespace OpenSim.Region.Physics.OdePlugin
458 m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y; 452 m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y;
459 if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z)) 453 if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z))
460 m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z; 454 m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z;
461 455
462 // decay applied velocity 456 // decay applied velocity
463 Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep))); 457 Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep)));
464 //Console.WriteLine("decay: " + decayfraction); 458 //Console.WriteLine("decay: " + decayfraction);
@@ -466,194 +460,192 @@ namespace OpenSim.Region.Physics.OdePlugin
466 //Console.WriteLine("actual: " + m_linearMotorDirection); 460 //Console.WriteLine("actual: " + m_linearMotorDirection);
467 } 461 }
468 else 462 else
469 { // requested is not significant 463 { // requested is not significant
470 // if what remains of applied is small, zero it. 464 // if what remains of applied is small, zero it.
471 if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f)) 465 if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
472 m_lastLinearVelocityVector = Vector3.Zero; 466 m_lastLinearVelocityVector = Vector3.Zero;
473 } 467 }
474 468
475 469 // convert requested object velocity to world-referenced vector
476 // convert requested object velocity to world-referenced vector
477 m_dir = m_lastLinearVelocityVector; 470 m_dir = m_lastLinearVelocityVector;
478 d.Quaternion rot = d.BodyGetQuaternion(Body); 471 d.Quaternion rot = d.BodyGetQuaternion(Body);
479 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object 472 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
480 m_dir *= rotq; // apply obj rotation to velocity vector 473 m_dir *= rotq; // apply obj rotation to velocity vector
481 474
482 // add Gravity andBuoyancy 475 // add Gravity andBuoyancy
483 // KF: So far I have found no good method to combine a script-requested 476 // KF: So far I have found no good method to combine a script-requested
484 // .Z velocity and gravity. Therefore only 0g will used script-requested 477 // .Z velocity and gravity. Therefore only 0g will used script-requested
485 // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only. 478 // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only.
486 Vector3 grav = Vector3.Zero; 479 Vector3 grav = Vector3.Zero;
487 if(m_VehicleBuoyancy < 1.0f) 480 if (m_VehicleBuoyancy < 1.0f)
488 { 481 {
489 // There is some gravity, make a gravity force vector 482 // There is some gravity, make a gravity force vector
490 // that is applied after object velocity. 483 // that is applied after object velocity.
491 d.Mass objMass; 484 d.Mass objMass;
492 d.BodyGetMass(Body, out objMass); 485 d.BodyGetMass(Body, out objMass);
493 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; 486 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
494 grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); 487 grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy);
495 // Preserve the current Z velocity 488 // Preserve the current Z velocity
496 d.Vector3 vel_now = d.BodyGetLinearVel(Body); 489 d.Vector3 vel_now = d.BodyGetLinearVel(Body);
497 m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity 490 m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity
498 } // else its 1.0, no gravity. 491 } // else its 1.0, no gravity.
499 492
500 // Check if hovering 493 // Check if hovering
501 if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) 494 if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
502 { 495 {
503 // We should hover, get the target height 496 // We should hover, get the target height
504 d.Vector3 pos = d.BodyGetPosition(Body); 497 d.Vector3 pos = d.BodyGetPosition(Body);
505 if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY) 498 if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
506 { 499 {
507 m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight; 500 m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight;
508 } 501 }
509 else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY) 502 else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
510 { 503 {
511 m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight; 504 m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
512 } 505 }
513 else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT) 506 else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
514 { 507 {
515 m_VhoverTargetHeight = m_VhoverHeight; 508 m_VhoverTargetHeight = m_VhoverHeight;
516 } 509 }
517 510
518 if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY) 511 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
519 { 512 {
520 // If body is aready heigher, use its height as target height 513 // If body is aready heigher, use its height as target height
521 if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z; 514 if (pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
522 } 515 }
523 516
524// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped 517// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
525// m_VhoverTimescale = 0f; // time to acheive height 518// m_VhoverTimescale = 0f; // time to acheive height
526// pTimestep is time since last frame,in secs 519// pTimestep is time since last frame,in secs
527 float herr0 = pos.Z - m_VhoverTargetHeight; 520 float herr0 = pos.Z - m_VhoverTargetHeight;
528 // Replace Vertical speed with correction figure if significant 521 // Replace Vertical speed with correction figure if significant
529 if(Math.Abs(herr0) > 0.01f ) 522 if (Math.Abs(herr0) > 0.01f)
530 { 523 {
531 d.Mass objMass; 524 d.Mass objMass;
532 d.BodyGetMass(Body, out objMass); 525 d.BodyGetMass(Body, out objMass);
533 m_dir.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale); 526 m_dir.Z = - ((herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
534 //KF: m_VhoverEfficiency is not yet implemented 527 //KF: m_VhoverEfficiency is not yet implemented
535 } 528 }
536 else 529 else
537 { 530 {
538 m_dir.Z = 0f; 531 m_dir.Z = 0f;
539 } 532 }
540 } 533 }
541 534
542 // Apply velocity 535 // Apply velocity
543 d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z); 536 d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z);
544 // apply gravity force 537 // apply gravity force
545 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); 538 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
546 539
547 540
548 // apply friction 541 // apply friction
549 Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep); 542 Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
550 m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount; 543 m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
551 } // end MoveLinear() 544 } // end MoveLinear()
552 545
553 private void MoveAngular(float pTimestep) 546 private void MoveAngular(float pTimestep)
554 { 547 {
555 /* 548 /*
556 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor 549 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
557 private int m_angularMotorApply = 0; // application frame counter 550 private int m_angularMotorApply = 0; // application frame counter
558 private float m_angularMotorVelocity = 0; // current angular motor velocity (ramps up and down) 551 private float m_angularMotorVelocity = 0; // current angular motor velocity (ramps up and down)
559 private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate 552 private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate
560 private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate 553 private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate
561 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate 554 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
562 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body 555 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
563 */ 556 */
564 557
565 // Get what the body is doing, this includes 'external' influences 558 // Get what the body is doing, this includes 'external' influences
566 d.Vector3 angularVelocity = d.BodyGetAngularVel(Body); 559 d.Vector3 angularVelocity = d.BodyGetAngularVel(Body);
567 // Vector3 angularVelocity = Vector3.Zero; 560 // Vector3 angularVelocity = Vector3.Zero;
568 561
569 if (m_angularMotorApply > 0) 562 if (m_angularMotorApply > 0)
570 { 563 {
571 // ramp up to new value 564 // ramp up to new value
572 // current velocity += error / ( time to get there / step interval ) 565 // current velocity += error / (time to get there / step interval)
573 // requested speed - last motor speed 566 // requested speed - last motor speed
574 m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep); 567 m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep);
575 m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep); 568 m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep);
576 m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep); 569 m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep);
577 570
578 m_angularMotorApply--; // This is done so that if script request rate is less than phys frame rate the expected 571 m_angularMotorApply--; // This is done so that if script request rate is less than phys frame rate the expected
579 // velocity may still be acheived. 572 // velocity may still be acheived.
580 } 573 }
581 else 574 else
582 { 575 {
583 // no motor recently applied, keep the body velocity 576 // no motor recently applied, keep the body velocity
584 /* m_angularMotorVelocity.X = angularVelocity.X; 577 /* m_angularMotorVelocity.X = angularVelocity.X;
585 m_angularMotorVelocity.Y = angularVelocity.Y; 578 m_angularMotorVelocity.Y = angularVelocity.Y;
586 m_angularMotorVelocity.Z = angularVelocity.Z; */ 579 m_angularMotorVelocity.Z = angularVelocity.Z; */
587 580
588 // and decay the velocity 581 // and decay the velocity
589 m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep); 582 m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep);
590 } // end motor section 583 } // end motor section
591
592 584
593 // Vertical attractor section 585 // Vertical attractor section
594 Vector3 vertattr = Vector3.Zero; 586 Vector3 vertattr = Vector3.Zero;
595 587
596 if(m_verticalAttractionTimescale < 300) 588 if (m_verticalAttractionTimescale < 300)
597 {
598 float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
599 // get present body rotation
600 d.Quaternion rot = d.BodyGetQuaternion(Body);
601 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
602 // make a vector pointing up
603 Vector3 verterr = Vector3.Zero;
604 verterr.Z = 1.0f;
605 // rotate it to Body Angle
606 verterr = verterr * rotq;
607 // 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.
608 // 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
609 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
610 if (verterr.Z < 0.0f)
611 {
612 verterr.X = 2.0f - verterr.X;
613 verterr.Y = 2.0f - verterr.Y;
614 }
615 // Error is 0 (no error) to +/- 2 (max error)
616 // scale it by VAservo
617 verterr = verterr * VAservo;
618//if(frcount == 0) Console.WriteLine("VAerr=" + verterr);
619
620 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
621 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
622 vertattr.X = verterr.Y;
623 vertattr.Y = - verterr.X;
624 vertattr.Z = 0f;
625
626 // scaling appears better usingsquare-law
627 float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
628 vertattr.X += bounce * angularVelocity.X;
629 vertattr.Y += bounce * angularVelocity.Y;
630
631 } // else vertical attractor is off
632
633 // m_lastVertAttractor = vertattr;
634
635 // Bank section tba
636 // Deflection section tba
637
638 // Sum velocities
639 m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection
640
641 if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
642 { 589 {
643 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); 590 float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
644 } 591 // get present body rotation
645 else 592 d.Quaternion rot = d.BodyGetQuaternion(Body);
646 { 593 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
647 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero. 594 // make a vector pointing up
648 } 595 Vector3 verterr = Vector3.Zero;
649 596 verterr.Z = 1.0f;
650 // apply friction 597 // rotate it to Body Angle
598 verterr = verterr * rotq;
599 // 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.
600 // 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
601 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
602 if (verterr.Z < 0.0f)
603 {
604 verterr.X = 2.0f - verterr.X;
605 verterr.Y = 2.0f - verterr.Y;
606 }
607 // Error is 0 (no error) to +/- 2 (max error)
608 // scale it by VAservo
609 verterr = verterr * VAservo;
610//if (frcount == 0) Console.WriteLine("VAerr=" + verterr);
611
612 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
613 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
614 vertattr.X = verterr.Y;
615 vertattr.Y = - verterr.X;
616 vertattr.Z = 0f;
617
618 // scaling appears better usingsquare-law
619 float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
620 vertattr.X += bounce * angularVelocity.X;
621 vertattr.Y += bounce * angularVelocity.Y;
622
623 } // else vertical attractor is off
624
625 // m_lastVertAttractor = vertattr;
626
627 // Bank section tba
628 // Deflection section tba
629
630 // Sum velocities
631 m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection
632
633 if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
634 {
635 if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
636 }
637 else
638 {
639 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
640 }
641
642 // apply friction
651 Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep); 643 Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
652 m_lastAngularVelocity -= m_lastAngularVelocity * decayamount; 644 m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;
653 645
654 // Apply to the body 646 // Apply to the body
655 d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); 647 d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
656 648
657 } //end MoveAngular 649 } //end MoveAngular
658 } 650 }
659} 651}
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index 3eb3b28..973aa84 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -1,5 +1,7 @@
1/* Copyright (c) Contributors, http://opensimulator.org/ 1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
2 * See CONTRIBUTORS.TXT for a full list of copyright holders. 3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
3 * Redistribution and use in source and binary forms, with or without 5 * Redistribution and use in source and binary forms, with or without
4 * modification, are permitted provided that the following conditions are met: 6 * modification, are permitted provided that the following conditions are met:
5 * * Redistributions of source code must retain the above copyright 7 * * Redistributions of source code must retain the above copyright
@@ -93,7 +95,7 @@ namespace OpenSim.Region.Physics.OdePlugin
93 private float m_targetHoverHeight; 95 private float m_targetHoverHeight;
94 private float m_groundHeight; 96 private float m_groundHeight;
95 private float m_waterHeight; 97 private float m_waterHeight;
96 private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. 98 private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
97 99
98 // private float m_tensor = 5f; 100 // private float m_tensor = 5f;
99 private int body_autodisable_frames = 20; 101 private int body_autodisable_frames = 20;
@@ -294,7 +296,7 @@ namespace OpenSim.Region.Physics.OdePlugin
294 m_taintselected = value; 296 m_taintselected = value;
295 m_isSelected = value; 297 m_isSelected = value;
296 } 298 }
297 if(m_isSelected) disableBodySoft(); 299 if (m_isSelected) disableBodySoft();
298 } 300 }
299 } 301 }
300 302
@@ -302,7 +304,7 @@ namespace OpenSim.Region.Physics.OdePlugin
302 { 304 {
303 prev_geom = prim_geom; 305 prev_geom = prim_geom;
304 prim_geom = geom; 306 prim_geom = geom;
305//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName); 307//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName);
306 if (prim_geom != IntPtr.Zero) 308 if (prim_geom != IntPtr.Zero)
307 { 309 {
308 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 310 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
@@ -314,7 +316,7 @@ namespace OpenSim.Region.Physics.OdePlugin
314 if (_parent != null && _parent is OdePrim) 316 if (_parent != null && _parent is OdePrim)
315 { 317 {
316 OdePrim parent = (OdePrim)_parent; 318 OdePrim parent = (OdePrim)_parent;
317//Console.WriteLine("SetGeom calls ChildSetGeom"); 319//Console.WriteLine("SetGeom calls ChildSetGeom");
318 parent.ChildSetGeom(this); 320 parent.ChildSetGeom(this);
319 } 321 }
320 } 322 }
@@ -331,7 +333,7 @@ namespace OpenSim.Region.Physics.OdePlugin
331 { 333 {
332 d.BodyEnable(Body); 334 d.BodyEnable(Body);
333 if (m_vehicle.Type != Vehicle.TYPE_NONE) 335 if (m_vehicle.Type != Vehicle.TYPE_NONE)
334 m_vehicle.Enable(Body, _parent_scene); 336 m_vehicle.Enable(Body, _parent_scene);
335 } 337 }
336 338
337 m_disabled = false; 339 m_disabled = false;
@@ -376,7 +378,7 @@ namespace OpenSim.Region.Physics.OdePlugin
376 d.BodySetAutoDisableSteps(Body, body_autodisable_frames); 378 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
377 379
378 // disconnect from world gravity so we can apply buoyancy 380 // disconnect from world gravity so we can apply buoyancy
379 d.BodySetGravityMode (Body, false); 381 d.BodySetGravityMode (Body, false);
380 382
381 m_interpenetrationcount = 0; 383 m_interpenetrationcount = 0;
382 m_collisionscore = 0; 384 m_collisionscore = 0;
@@ -872,7 +874,7 @@ namespace OpenSim.Region.Physics.OdePlugin
872 874
873 public void ProcessTaints(float timestep) 875 public void ProcessTaints(float timestep)
874 { 876 {
875//Console.WriteLine("ProcessTaints for " + m_primName ); 877//Console.WriteLine("ProcessTaints for " + m_primName);
876 if (m_taintadd) 878 if (m_taintadd)
877 { 879 {
878 changeadd(timestep); 880 changeadd(timestep);
@@ -880,24 +882,24 @@ namespace OpenSim.Region.Physics.OdePlugin
880 882
881 if (prim_geom != IntPtr.Zero) 883 if (prim_geom != IntPtr.Zero)
882 { 884 {
883 if (!_position.ApproxEquals(m_taintposition, 0f)) 885 if (!_position.ApproxEquals(m_taintposition, 0f))
884 changemove(timestep); 886 changemove(timestep);
885 887
886 if (m_taintrot != _orientation) 888 if (m_taintrot != _orientation)
887 { 889 {
888 if(childPrim && IsPhysical) // For physical child prim... 890 if (childPrim && IsPhysical) // For physical child prim...
889 { 891 {
890 rotate(timestep); 892 rotate(timestep);
891 // KF: ODE will also rotate the parent prim! 893 // KF: ODE will also rotate the parent prim!
892 // so rotate the root back to where it was 894 // so rotate the root back to where it was
893 OdePrim parent = (OdePrim)_parent; 895 OdePrim parent = (OdePrim)_parent;
894 parent.rotate(timestep); 896 parent.rotate(timestep);
895 } 897 }
896 else 898 else
897 { 899 {
898 //Just rotate the prim 900 //Just rotate the prim
899 rotate(timestep); 901 rotate(timestep);
900 } 902 }
901 } 903 }
902 // 904 //
903 905
@@ -1006,7 +1008,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1006 // destroy link 1008 // destroy link
1007 else if (_parent != null && m_taintparent == null) 1009 else if (_parent != null && m_taintparent == null)
1008 { 1010 {
1009//Console.WriteLine(" changelink B"); 1011//Console.WriteLine(" changelink B");
1010 1012
1011 if (_parent is OdePrim) 1013 if (_parent is OdePrim)
1012 { 1014 {
@@ -1033,7 +1035,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1033 // prim is the child 1035 // prim is the child
1034 public void ParentPrim(OdePrim prim) 1036 public void ParentPrim(OdePrim prim)
1035 { 1037 {
1036//Console.WriteLine("ParentPrim " + m_primName); 1038//Console.WriteLine("ParentPrim " + m_primName);
1037 if (this.m_localID != prim.m_localID) 1039 if (this.m_localID != prim.m_localID)
1038 { 1040 {
1039 if (Body == IntPtr.Zero) 1041 if (Body == IntPtr.Zero)
@@ -1047,7 +1049,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1047 { 1049 {
1048 if (!childrenPrim.Contains(prim)) 1050 if (!childrenPrim.Contains(prim))
1049 { 1051 {
1050//Console.WriteLine("childrenPrim.Add " + prim); 1052//Console.WriteLine("childrenPrim.Add " + prim);
1051 childrenPrim.Add(prim); 1053 childrenPrim.Add(prim);
1052 1054
1053 foreach (OdePrim prm in childrenPrim) 1055 foreach (OdePrim prm in childrenPrim)
@@ -1080,7 +1082,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1080 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet"); 1082 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet");
1081 continue; 1083 continue;
1082 } 1084 }
1083//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + m_primName); 1085//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + m_primName);
1084 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); 1086 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
1085 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); 1087 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
1086 1088
@@ -1128,7 +1130,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1128 m_collisionCategories |= CollisionCategories.Body; 1130 m_collisionCategories |= CollisionCategories.Body;
1129 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); 1131 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1130 1132
1131//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + m_primName); 1133//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + m_primName);
1132 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 1134 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1133//Console.WriteLine(" Post GeomSetCategoryBits 2"); 1135//Console.WriteLine(" Post GeomSetCategoryBits 2");
1134 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 1136 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
@@ -1203,7 +1205,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1203 { 1205 {
1204 foreach (OdePrim prm in childrenPrim) 1206 foreach (OdePrim prm in childrenPrim)
1205 { 1207 {
1206//Console.WriteLine("ChildSetGeom calls ParentPrim"); 1208//Console.WriteLine("ChildSetGeom calls ParentPrim");
1207 ParentPrim(prm); 1209 ParentPrim(prm);
1208 } 1210 }
1209 } 1211 }
@@ -1230,7 +1232,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1230 1232
1231 lock (childrenPrim) 1233 lock (childrenPrim)
1232 { 1234 {
1233 //Console.WriteLine("childrenPrim.Remove " + odePrim); 1235 //Console.WriteLine("childrenPrim.Remove " + odePrim);
1234 childrenPrim.Remove(odePrim); 1236 childrenPrim.Remove(odePrim);
1235 } 1237 }
1236 1238
@@ -1248,7 +1250,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1248 { 1250 {
1249 foreach (OdePrim prm in childrenPrim) 1251 foreach (OdePrim prm in childrenPrim)
1250 { 1252 {
1251//Console.WriteLine("ChildDelink calls ParentPrim"); 1253//Console.WriteLine("ChildDelink calls ParentPrim");
1252 ParentPrim(prm); 1254 ParentPrim(prm);
1253 } 1255 }
1254 } 1256 }
@@ -1350,7 +1352,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1350 1352
1351 public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh) 1353 public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh)
1352 { 1354 {
1353//Console.WriteLine("CreateGeom:"); 1355//Console.WriteLine("CreateGeom:");
1354 if (_mesh != null) 1356 if (_mesh != null)
1355 { 1357 {
1356 setMesh(_parent_scene, _mesh); 1358 setMesh(_parent_scene, _mesh);
@@ -1381,7 +1383,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1381 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1383 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1382 try 1384 try
1383 { 1385 {
1384//Console.WriteLine(" CreateGeom 2"); 1386//Console.WriteLine(" CreateGeom 2");
1385 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); 1387 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1386 } 1388 }
1387 catch (AccessViolationException) 1389 catch (AccessViolationException)
@@ -1397,7 +1399,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1397 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1399 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1398 try 1400 try
1399 { 1401 {
1400//Console.WriteLine(" CreateGeom 3"); 1402//Console.WriteLine(" CreateGeom 3");
1401 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); 1403 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1402 } 1404 }
1403 catch (AccessViolationException) 1405 catch (AccessViolationException)
@@ -1414,7 +1416,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1414 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1416 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1415 try 1417 try
1416 { 1418 {
1417//Console.WriteLine(" CreateGeom 4"); 1419//Console.WriteLine(" CreateGeom 4");
1418 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); 1420 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1419 } 1421 }
1420 catch (AccessViolationException) 1422 catch (AccessViolationException)
@@ -1451,7 +1453,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1451 1453
1452 lock (_parent_scene.OdeLock) 1454 lock (_parent_scene.OdeLock)
1453 { 1455 {
1454//Console.WriteLine("changeadd 1"); 1456//Console.WriteLine("changeadd 1");
1455 CreateGeom(m_targetSpace, _mesh); 1457 CreateGeom(m_targetSpace, _mesh);
1456 1458
1457 if (prim_geom != IntPtr.Zero) 1459 if (prim_geom != IntPtr.Zero)
@@ -1508,7 +1510,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1508 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body) 1510 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
1509 { 1511 {
1510// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used?? 1512// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
1511Console.WriteLine(" JointCreateFixed"); 1513Console.WriteLine(" JointCreateFixed");
1512 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); 1514 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
1513 d.JointAttach(m_linkJoint, Body, odParent.Body); 1515 d.JointAttach(m_linkJoint, Body, odParent.Body);
1514 d.JointSetFixed(m_linkJoint); 1516 d.JointSetFixed(m_linkJoint);
@@ -1562,244 +1564,244 @@ Console.WriteLine(" JointCreateFixed");
1562 float fz = 0; 1564 float fz = 0;
1563 1565
1564 1566
1565 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. 1567 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
1566 { 1568 {
1567 if (m_vehicle.Type != Vehicle.TYPE_NONE) 1569 if (m_vehicle.Type != Vehicle.TYPE_NONE)
1568 { 1570 {
1569 // 'VEHICLES' are dealt with in ODEDynamics.cs 1571 // 'VEHICLES' are dealt with in ODEDynamics.cs
1570 m_vehicle.Step(timestep, _parent_scene); 1572 m_vehicle.Step(timestep, _parent_scene);
1571 } 1573 }
1572 else 1574 else
1573 { 1575 {
1574//Console.WriteLine("Move " + m_primName); 1576//Console.WriteLine("Move " + m_primName);
1575 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 1577 if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
1576 // NON-'VEHICLES' are dealt with here 1578 // NON-'VEHICLES' are dealt with here
1577 if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f)) 1579 if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f))
1578 { 1580 {
1579 d.Vector3 avel2 = d.BodyGetAngularVel(Body); 1581 d.Vector3 avel2 = d.BodyGetAngularVel(Body);
1580 if (m_angularlock.X == 1) 1582 if (m_angularlock.X == 1)
1581 avel2.X = 0; 1583 avel2.X = 0;
1582 if (m_angularlock.Y == 1) 1584 if (m_angularlock.Y == 1)
1583 avel2.Y = 0; 1585 avel2.Y = 0;
1584 if (m_angularlock.Z == 1) 1586 if (m_angularlock.Z == 1)
1585 avel2.Z = 0; 1587 avel2.Z = 0;
1586 d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z); 1588 d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
1587 } 1589 }
1588 //float PID_P = 900.0f; 1590 //float PID_P = 900.0f;
1589 1591
1590 float m_mass = CalculateMass(); 1592 float m_mass = CalculateMass();
1591 1593
1592// fz = 0f; 1594// fz = 0f;
1593 //m_log.Info(m_collisionFlags.ToString()); 1595 //m_log.Info(m_collisionFlags.ToString());
1594 1596
1595 1597
1596 //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. 1598 //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
1597 // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ?? 1599 // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ??
1598 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up 1600 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
1599 // gravityz multiplier = 1 - m_buoyancy 1601 // gravityz multiplier = 1 - m_buoyancy
1600 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; 1602 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass;
1601 1603
1602 if (m_usePID) 1604 if (m_usePID)
1603 { 1605 {
1604//Console.WriteLine("PID " + m_primName); 1606//Console.WriteLine("PID " + m_primName);
1605 // KF - this is for object move? eg. llSetPos() ? 1607 // KF - this is for object move? eg. llSetPos() ?
1606 //if (!d.BodyIsEnabled(Body)) 1608 //if (!d.BodyIsEnabled(Body))
1607 //d.BodySetForce(Body, 0f, 0f, 0f); 1609 //d.BodySetForce(Body, 0f, 0f, 0f);
1608 // If we're using the PID controller, then we have no gravity 1610 // If we're using the PID controller, then we have no gravity
1609 //fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply... 1611 //fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply...
1610 fz = 0f; 1612 fz = 0f;
1611 1613
1612 // no lock; for now it's only called from within Simulate() 1614 // no lock; for now it's only called from within Simulate()
1613 1615
1614 // If the PID Controller isn't active then we set our force 1616 // If the PID Controller isn't active then we set our force
1615 // calculating base velocity to the current position 1617 // calculating base velocity to the current position
1616 1618
1617 if ((m_PIDTau < 1) && (m_PIDTau != 0)) 1619 if ((m_PIDTau < 1) && (m_PIDTau != 0))
1618 { 1620 {
1619 //PID_G = PID_G / m_PIDTau; 1621 //PID_G = PID_G / m_PIDTau;
1620 m_PIDTau = 1; 1622 m_PIDTau = 1;
1621 } 1623 }
1622 1624
1623 if ((PID_G - m_PIDTau) <= 0) 1625 if ((PID_G - m_PIDTau) <= 0)
1624 { 1626 {
1625 PID_G = m_PIDTau + 1; 1627 PID_G = m_PIDTau + 1;
1626 } 1628 }
1627 //PidStatus = true; 1629 //PidStatus = true;
1628 1630
1629 // PhysicsVector vec = new PhysicsVector(); 1631 // PhysicsVector vec = new PhysicsVector();
1630 d.Vector3 vel = d.BodyGetLinearVel(Body); 1632 d.Vector3 vel = d.BodyGetLinearVel(Body);
1631 1633
1632 d.Vector3 pos = d.BodyGetPosition(Body); 1634 d.Vector3 pos = d.BodyGetPosition(Body);
1633 _target_velocity = 1635 _target_velocity =
1634 new Vector3( 1636 new Vector3(
1635 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), 1637 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
1636 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), 1638 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
1637 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) 1639 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
1638 ); 1640 );
1639 1641
1640 // if velocity is zero, use position control; otherwise, velocity control 1642 // if velocity is zero, use position control; otherwise, velocity control
1641 1643
1642 if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f)) 1644 if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
1643 { 1645 {
1644 // keep track of where we stopped. No more slippin' & slidin' 1646 // keep track of where we stopped. No more slippin' & slidin'
1645 1647
1646 // We only want to deactivate the PID Controller if we think we want to have our surrogate 1648 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1647 // react to the physics scene by moving it's position. 1649 // react to the physics scene by moving it's position.
1648 // Avatar to Avatar collisions 1650 // Avatar to Avatar collisions
1649 // Prim to avatar collisions 1651 // Prim to avatar collisions
1650 1652
1651 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); 1653 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
1652 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); 1654 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
1653 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; 1655 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
1654 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); 1656 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
1655 d.BodySetLinearVel(Body, 0, 0, 0); 1657 d.BodySetLinearVel(Body, 0, 0, 0);
1656 d.BodyAddForce(Body, 0, 0, fz); 1658 d.BodyAddForce(Body, 0, 0, fz);
1657 return; 1659 return;
1658 } 1660 }
1659 else 1661 else
1660 { 1662 {
1661 _zeroFlag = false; 1663 _zeroFlag = false;
1662 1664
1663 // We're flying and colliding with something 1665 // We're flying and colliding with something
1664 fx = ((_target_velocity.X) - vel.X) * (PID_D); 1666 fx = ((_target_velocity.X) - vel.X) * (PID_D);
1665 fy = ((_target_velocity.Y) - vel.Y) * (PID_D); 1667 fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
1666 1668
1667 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; 1669 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
1668 1670
1669 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); 1671 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1670 } 1672 }
1671 } // end if (m_usePID) 1673 } // end if (m_usePID)
1672 1674
1673 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller 1675 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
1674 if (m_useHoverPID && !m_usePID) 1676 if (m_useHoverPID && !m_usePID)
1675 { 1677 {
1676//Console.WriteLine("Hover " + m_primName); 1678//Console.WriteLine("Hover " + m_primName);
1677
1678 // If we're using the PID controller, then we have no gravity
1679 fz = (-1 * _parent_scene.gravityz) * m_mass;
1680
1681 // no lock; for now it's only called from within Simulate()
1682
1683 // If the PID Controller isn't active then we set our force
1684 // calculating base velocity to the current position
1685
1686 if ((m_PIDTau < 1))
1687 {
1688 PID_G = PID_G / m_PIDTau;
1689 }
1690
1691 if ((PID_G - m_PIDTau) <= 0)
1692 {
1693 PID_G = m_PIDTau + 1;
1694 }
1695 1679
1680 // If we're using the PID controller, then we have no gravity
1681 fz = (-1 * _parent_scene.gravityz) * m_mass;
1682
1683 // no lock; for now it's only called from within Simulate()
1696 1684
1697 // Where are we, and where are we headed? 1685 // If the PID Controller isn't active then we set our force
1698 d.Vector3 pos = d.BodyGetPosition(Body); 1686 // calculating base velocity to the current position
1699 d.Vector3 vel = d.BodyGetLinearVel(Body); 1687
1700 1688 if ((m_PIDTau < 1))
1701 1689 {
1702 // Non-Vehicles have a limited set of Hover options. 1690 PID_G = PID_G / m_PIDTau;
1703 // determine what our target height really is based on HoverType 1691 }
1704 switch (m_PIDHoverType) 1692
1705 { 1693 if ((PID_G - m_PIDTau) <= 0)
1706 case PIDHoverType.Ground: 1694 {
1707 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); 1695 PID_G = m_PIDTau + 1;
1708 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; 1696 }
1709 break; 1697
1710 case PIDHoverType.GroundAndWater: 1698
1711 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); 1699 // Where are we, and where are we headed?
1712 m_waterHeight = _parent_scene.GetWaterLevel(); 1700 d.Vector3 pos = d.BodyGetPosition(Body);
1713 if (m_groundHeight > m_waterHeight) 1701 d.Vector3 vel = d.BodyGetLinearVel(Body);
1714 { 1702
1715 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; 1703
1716 } 1704 // Non-Vehicles have a limited set of Hover options.
1717 else 1705 // determine what our target height really is based on HoverType
1718 { 1706 switch (m_PIDHoverType)
1719 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; 1707 {
1720 } 1708 case PIDHoverType.Ground:
1721 break; 1709 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1722 1710 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1723 } // end switch (m_PIDHoverType) 1711 break;
1724 1712 case PIDHoverType.GroundAndWater:
1725 1713 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1726 _target_velocity = 1714 m_waterHeight = _parent_scene.GetWaterLevel();
1715 if (m_groundHeight > m_waterHeight)
1716 {
1717 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1718 }
1719 else
1720 {
1721 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
1722 }
1723 break;
1724
1725 } // end switch (m_PIDHoverType)
1726
1727
1728 _target_velocity =
1727 new Vector3(0.0f, 0.0f, 1729 new Vector3(0.0f, 0.0f,
1728 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) 1730 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
1729 ); 1731 );
1730 1732
1731 // if velocity is zero, use position control; otherwise, velocity control 1733 // if velocity is zero, use position control; otherwise, velocity control
1732 1734
1733 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) 1735 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
1734 { 1736 {
1735 // keep track of where we stopped. No more slippin' & slidin' 1737 // keep track of where we stopped. No more slippin' & slidin'
1736 1738
1737 // We only want to deactivate the PID Controller if we think we want to have our surrogate 1739 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1738 // react to the physics scene by moving it's position. 1740 // react to the physics scene by moving it's position.
1739 // Avatar to Avatar collisions 1741 // Avatar to Avatar collisions
1740 // Prim to avatar collisions 1742 // Prim to avatar collisions
1741 1743
1742 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); 1744 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
1743 d.BodySetLinearVel(Body, vel.X, vel.Y, 0); 1745 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
1744 d.BodyAddForce(Body, 0, 0, fz); 1746 d.BodyAddForce(Body, 0, 0, fz);
1745 return; 1747 return;
1746 } 1748 }
1747 else 1749 else
1748 { 1750 {
1749 _zeroFlag = false; 1751 _zeroFlag = false;
1750 1752
1751 // We're flying and colliding with something 1753 // We're flying and colliding with something
1752 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); 1754 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1753 } 1755 }
1754 } 1756 }
1755 1757
1756 fx *= m_mass; 1758 fx *= m_mass;
1757 fy *= m_mass; 1759 fy *= m_mass;
1758 //fz *= m_mass; 1760 //fz *= m_mass;
1759 1761
1760 fx += m_force.X; 1762 fx += m_force.X;
1761 fy += m_force.Y; 1763 fy += m_force.Y;
1762 fz += m_force.Z; 1764 fz += m_force.Z;
1763 1765
1764 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); 1766 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
1765 if (fx != 0 || fy != 0 || fz != 0) 1767 if (fx != 0 || fy != 0 || fz != 0)
1766 { 1768 {
1767 //m_taintdisable = true; 1769 //m_taintdisable = true;
1768 //base.RaiseOutOfBounds(Position); 1770 //base.RaiseOutOfBounds(Position);
1769 //d.BodySetLinearVel(Body, fx, fy, 0f); 1771 //d.BodySetLinearVel(Body, fx, fy, 0f);
1770 if (!d.BodyIsEnabled(Body)) 1772 if (!d.BodyIsEnabled(Body))
1771 { 1773 {
1772 // A physical body at rest on a surface will auto-disable after a while, 1774 // A physical body at rest on a surface will auto-disable after a while,
1773 // this appears to re-enable it incase the surface it is upon vanishes, 1775 // this appears to re-enable it incase the surface it is upon vanishes,
1774 // and the body should fall again. 1776 // and the body should fall again.
1775 d.BodySetLinearVel(Body, 0f, 0f, 0f); 1777 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1776 d.BodySetForce(Body, 0, 0, 0); 1778 d.BodySetForce(Body, 0, 0, 0);
1777 enableBodySoft(); 1779 enableBodySoft();
1778 } 1780 }
1779 1781
1780 // 35x10 = 350n times the mass per second applied maximum. 1782 // 35x10 = 350n times the mass per second applied maximum.
1781 float nmax = 35f * m_mass; 1783 float nmax = 35f * m_mass;
1782 float nmin = -35f * m_mass; 1784 float nmin = -35f * m_mass;
1783 1785
1784 1786
1785 if (fx > nmax) 1787 if (fx > nmax)
1786 fx = nmax; 1788 fx = nmax;
1787 if (fx < nmin) 1789 if (fx < nmin)
1788 fx = nmin; 1790 fx = nmin;
1789 if (fy > nmax) 1791 if (fy > nmax)
1790 fy = nmax; 1792 fy = nmax;
1791 if (fy < nmin) 1793 if (fy < nmin)
1792 fy = nmin; 1794 fy = nmin;
1793 d.BodyAddForce(Body, fx, fy, fz); 1795 d.BodyAddForce(Body, fx, fy, fz);
1794//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); 1796//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
1795 } 1797 }
1796 } 1798 }
1797 } 1799 }
1798 else 1800 else
1799 { // is not physical, or is not a body or is selected 1801 { // is not physical, or is not a body or is selected
1800 // _zeroPosition = d.BodyGetPosition(Body); 1802 // _zeroPosition = d.BodyGetPosition(Body);
1801 return; 1803 return;
1802//Console.WriteLine("Nothing " + m_primName); 1804//Console.WriteLine("Nothing " + m_primName);
1803 1805
1804 } 1806 }
1805 } 1807 }
@@ -1815,18 +1817,18 @@ Console.WriteLine(" JointCreateFixed");
1815 myrot.W = _orientation.W; 1817 myrot.W = _orientation.W;
1816 if (Body != IntPtr.Zero) 1818 if (Body != IntPtr.Zero)
1817 { 1819 {
1818 // KF: If this is a root prim do BodySet 1820 // KF: If this is a root prim do BodySet
1819 d.BodySetQuaternion(Body, ref myrot); 1821 d.BodySetQuaternion(Body, ref myrot);
1820 if (m_isphysical) 1822 if (m_isphysical)
1821 { 1823 {
1822 if (!m_angularlock.ApproxEquals(Vector3.One, 0f)) 1824 if (!m_angularlock.ApproxEquals(Vector3.One, 0f))
1823 createAMotor(m_angularlock); 1825 createAMotor(m_angularlock);
1824 } 1826 }
1825 } 1827 }
1826 else 1828 else
1827 { 1829 {
1828 // daughter prim, do Geom set 1830 // daughter prim, do Geom set
1829 d.GeomSetQuaternion(prim_geom, ref myrot); 1831 d.GeomSetQuaternion(prim_geom, ref myrot);
1830 } 1832 }
1831 1833
1832 resetCollisionAccounting(); 1834 resetCollisionAccounting();
@@ -1890,7 +1892,7 @@ Console.WriteLine(" JointCreateFixed");
1890 m_log.Error("[PHYSICS]: PrimGeom dead"); 1892 m_log.Error("[PHYSICS]: PrimGeom dead");
1891 } 1893 }
1892 } 1894 }
1893//Console.WriteLine("changePhysicsStatus for " + m_primName ); 1895//Console.WriteLine("changePhysicsStatus for " + m_primName);
1894 changeadd(2f); 1896 changeadd(2f);
1895 } 1897 }
1896 if (childPrim) 1898 if (childPrim)
@@ -1976,7 +1978,7 @@ Console.WriteLine(" JointCreateFixed");
1976 else 1978 else
1977 { 1979 {
1978 _mesh = null; 1980 _mesh = null;
1979//Console.WriteLine("changesize 2"); 1981//Console.WriteLine("changesize 2");
1980 CreateGeom(m_targetSpace, _mesh); 1982 CreateGeom(m_targetSpace, _mesh);
1981 } 1983 }
1982 1984
@@ -2083,7 +2085,7 @@ Console.WriteLine(" JointCreateFixed");
2083 else 2085 else
2084 { 2086 {
2085 _mesh = null; 2087 _mesh = null;
2086//Console.WriteLine("changeshape"); 2088//Console.WriteLine("changeshape");
2087 CreateGeom(m_targetSpace, null); 2089 CreateGeom(m_targetSpace, null);
2088 } 2090 }
2089 2091
@@ -2454,7 +2456,7 @@ Console.WriteLine(" JointCreateFixed");
2454 if (QuaternionIsFinite(value)) 2456 if (QuaternionIsFinite(value))
2455 { 2457 {
2456 _orientation = value; 2458 _orientation = value;
2457 } 2459 }
2458 else 2460 else
2459 m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object"); 2461 m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object");
2460 2462
@@ -2675,8 +2677,8 @@ Console.WriteLine(" JointCreateFixed");
2675 //outofBounds = true; 2677 //outofBounds = true;
2676 } 2678 }
2677 2679
2678 //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)); 2680 //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation));
2679//Console.WriteLine("Adiff " + m_primName + " = " + Adiff); 2681//Console.WriteLine("Adiff " + m_primName + " = " + Adiff);
2680 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) 2682 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
2681 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) 2683 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
2682 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) 2684 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
@@ -2684,7 +2686,7 @@ Console.WriteLine(" JointCreateFixed");
2684 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large 2686 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large
2685 { 2687 {
2686 _zeroFlag = true; 2688 _zeroFlag = true;
2687//Console.WriteLine("ZFT 2"); 2689//Console.WriteLine("ZFT 2");
2688 m_throttleUpdates = false; 2690 m_throttleUpdates = false;
2689 } 2691 }
2690 else 2692 else
diff --git a/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs
index 7314107..ba77dae 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs
@@ -110,7 +110,7 @@ namespace OpenSim.Region.Physics.OdePlugin
110 for (int i = 0; i < reqs.Length; i++) 110 for (int i = 0; i < reqs.Length; i++)
111 { 111 {
112 if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast 112 if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast
113 RayCast(reqs[i]); // if there isn't anyone to send results 113 RayCast(reqs[i]); // if there isn't anyone to send results
114 } 114 }
115 /* 115 /*
116 foreach (ODERayCastRequest req in m_PendingRequests) 116 foreach (ODERayCastRequest req in m_PendingRequests)
diff --git a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
index 0384d6e..7984bd9 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
@@ -2152,7 +2152,7 @@ namespace OpenSim.Region.Physics.OdePlugin
2152 /// <param name="prim"></param> 2152 /// <param name="prim"></param>
2153 public void RemovePrimThreadLocked(OdePrim prim) 2153 public void RemovePrimThreadLocked(OdePrim prim)
2154 { 2154 {
2155//Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName); 2155//Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName);
2156 lock (prim) 2156 lock (prim)
2157 { 2157 {
2158 remCollisionEventReporting(prim); 2158 remCollisionEventReporting(prim);
@@ -2603,12 +2603,12 @@ namespace OpenSim.Region.Physics.OdePlugin
2603 lock (_taintedPrimLock) 2603 lock (_taintedPrimLock)
2604 { 2604 {
2605 if (!(_taintedPrimH.Contains(taintedprim))) 2605 if (!(_taintedPrimH.Contains(taintedprim)))
2606 { 2606 {
2607//Console.WriteLine("AddPhysicsActorTaint to " + taintedprim.m_primName); 2607//Console.WriteLine("AddPhysicsActorTaint to " + taintedprim.m_primName);
2608 _taintedPrimH.Add(taintedprim); // HashSet for searching 2608 _taintedPrimH.Add(taintedprim); // HashSet for searching
2609 _taintedPrimL.Add(taintedprim); // List for ordered readout 2609 _taintedPrimL.Add(taintedprim); // List for ordered readout
2610 } 2610 }
2611 } 2611 }
2612 return; 2612 return;
2613 } 2613 }
2614 else if (prim is OdeCharacter) 2614 else if (prim is OdeCharacter)
@@ -2736,12 +2736,12 @@ namespace OpenSim.Region.Physics.OdePlugin
2736 { 2736 {
2737 if (prim.m_taintremove) 2737 if (prim.m_taintremove)
2738 { 2738 {
2739 //Console.WriteLine("Simulate calls RemovePrimThreadLocked"); 2739 //Console.WriteLine("Simulate calls RemovePrimThreadLocked");
2740 RemovePrimThreadLocked(prim); 2740 RemovePrimThreadLocked(prim);
2741 } 2741 }
2742 else 2742 else
2743 { 2743 {
2744 //Console.WriteLine("Simulate calls ProcessTaints"); 2744 //Console.WriteLine("Simulate calls ProcessTaints");
2745 prim.ProcessTaints(timeStep); 2745 prim.ProcessTaints(timeStep);
2746 } 2746 }
2747 processedtaints = true; 2747 processedtaints = true;
@@ -2937,7 +2937,7 @@ namespace OpenSim.Region.Physics.OdePlugin
2937 } 2937 }
2938 2938
2939 if (processedtaints) 2939 if (processedtaints)
2940//Console.WriteLine("Simulate calls Clear of _taintedPrim list"); 2940//Console.WriteLine("Simulate calls Clear of _taintedPrim list");
2941 _taintedPrimH.Clear(); 2941 _taintedPrimH.Clear();
2942 _taintedPrimL.Clear(); 2942 _taintedPrimL.Clear();
2943 } 2943 }