ogb912/opensim
1
0
Fork 0
mirror of https://github.com/opensim/opensim.git synced 2026-08-13 05:05:43 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of ssh://3dhosting.de/var/git/careminster

Browse Source
This commit is contained in:
Kitto Flora
2010-01-03 23:10:01 -05:00
parent 3f901d313b 1121919b57
commit 253e2ed699
59 changed files with 2095 additions and 738 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsActor.cs
View File

@@ -305,22 +305,22 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin
public override Quaternion APIDTarget
{
set { return; }
set { return; }
}
public override bool APIDActive
{
set { return; }
set { return; }
}
public override float APIDStrength
{
set { return; }
set { return; }
}
public override float APIDDamping
{
set { return; }
set { return; }
}
public override void SubscribeEvents(int ms)

8
OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs
View File

@@ -622,22 +622,22 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
public override Quaternion APIDTarget
{
set { return; }
set { return; }
}
public override bool APIDActive
{
set { return; }
set { return; }
}
public override float APIDStrength
{
set { return; }
set { return; }
}
public override float APIDDamping
{
set { return; }
set { return; }
}
/// <summary>

2
OpenSim/Region/Physics/Manager/PhysicsActor.cs
View File

@@ -243,7 +243,7 @@ namespace OpenSim.Region.Physics.Manager
public abstract PIDHoverType PIDHoverType { set;}
public abstract float PIDHoverTau { set;}
// For RotLookAt
// For RotLookAt
public abstract Quaternion APIDTarget { set;}
public abstract bool APIDActive { set;}
public abstract float APIDStrength { set;}

436
OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs
View File

@@ -32,11 +32,11 @@
* ODEDynamics.cs contains methods dealing with Prim Physical motion
* (dynamics) and the associated settings. Old Linear and angular
* motors for dynamic motion have been replace with MoveLinear()
* and MoveAngular(); 'Physical' is used only to switch ODE dynamic
* and MoveAngular(); 'Physical' is used only to switch ODE dynamic
* simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
* switch between 'VEHICLE' parameter use and general dynamics
* settings use.
*/
*/
using System;
using System.Collections.Generic;
@@ -53,7 +53,7 @@ namespace OpenSim.Region.Physics.OdePlugin
public class ODEDynamics
{
public Vehicle Type
{
{
get { return m_type; }
}
@@ -62,14 +62,14 @@ namespace OpenSim.Region.Physics.OdePlugin
get { return m_body; }
}
private int frcount = 0; // Used to limit dynamics debug output to
// every 100th frame
private int frcount = 0; // Used to limit dynamics debug output to
// every 100th frame
// private OdeScene m_parentScene = null;
private IntPtr m_body = IntPtr.Zero;
// private IntPtr m_jointGroup = IntPtr.Zero;
// private IntPtr m_aMotor = IntPtr.Zero;
// Vehicle properties
private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
@@ -82,7 +82,7 @@ namespace OpenSim.Region.Physics.OdePlugin
// HOVER_UP_ONLY
// LIMIT_MOTOR_UP
// LIMIT_ROLL_ONLY
// Linear properties
private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time
private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL
@@ -91,47 +91,43 @@ namespace OpenSim.Region.Physics.OdePlugin
private float m_linearMotorDecayTimescale = 0;
private float m_linearMotorTimescale = 0;
private Vector3 m_lastLinearVelocityVector = Vector3.Zero;
// private bool m_LinearMotorSetLastFrame = false;
// private bool m_LinearMotorSetLastFrame = false;
// private Vector3 m_linearMotorOffset = Vector3.Zero;
//Angular properties
private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
private int m_angularMotorApply = 0; // application frame counter
private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity
private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity
private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate
private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate
private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
// private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body
//Deflection properties
//Deflection properties
// private float m_angularDeflectionEfficiency = 0;
// private float m_angularDeflectionTimescale = 0;
// private float m_linearDeflectionEfficiency = 0;
// private float m_linearDeflectionTimescale = 0;
//Banking properties
// private float m_bankingEfficiency = 0;
// private float m_bankingMix = 0;
// private float m_bankingTimescale = 0;
//Hover and Buoyancy properties
private float m_VhoverHeight = 0f;
// private float m_VhoverEfficiency = 0f;
private float m_VhoverTimescale = 0f;
private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle.
// Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
// KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
// Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
//Attractor properties
// Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
// KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
// Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
//Attractor properties
private float m_verticalAttractionEfficiency = 1.0f; // damped
private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor.
internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
{
@@ -166,13 +162,13 @@ namespace OpenSim.Region.Physics.OdePlugin
// m_bankingTimescale = pValue;
break;
case Vehicle.BUOYANCY:
if (pValue < -1f) pValue = -1f;
if (pValue > 1f) pValue = 1f;
if (pValue < -1f) pValue = -1f;
if (pValue > 1f) pValue = 1f;
m_VehicleBuoyancy = pValue;
break;
// case Vehicle.HOVER_EFFICIENCY:
// if (pValue < 0f) pValue = 0f;
// if (pValue > 1f) pValue = 1f;
// if (pValue < 0f) pValue = 0f;
// if (pValue > 1f) pValue = 1f;
// m_VhoverEfficiency = pValue;
// break;
case Vehicle.HOVER_HEIGHT:
@@ -199,7 +195,7 @@ namespace OpenSim.Region.Physics.OdePlugin
m_linearMotorTimescale = pValue;
break;
case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable
if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable
if (pValue > 1.0f) pValue = 1.0f;
m_verticalAttractionEfficiency = pValue;
break;
@@ -207,8 +203,8 @@ namespace OpenSim.Region.Physics.OdePlugin
if (pValue < 0.01f) pValue = 0.01f;
m_verticalAttractionTimescale = pValue;
break;
// These are vector properties but the engine lets you use a single float value to
// These are vector properties but the engine lets you use a single float value to
// set all of the components to the same value
case Vehicle.ANGULAR_FRICTION_TIMESCALE:
m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
@@ -229,7 +225,6 @@ namespace OpenSim.Region.Physics.OdePlugin
break;
}
}//end ProcessFloatVehicleParam
internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
@@ -242,12 +237,12 @@ namespace OpenSim.Region.Physics.OdePlugin
case Vehicle.ANGULAR_MOTOR_DIRECTION:
m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
// Limit requested angular speed to 2 rps= 4 pi rads/sec
if(m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f;
if(m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f;
if(m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f;
if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f;
if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f;
if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f;
if (m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f;
if (m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f;
if (m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f;
if (m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f;
if (m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f;
if (m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f;
m_angularMotorApply = 10;
break;
case Vehicle.LINEAR_FRICTION_TIMESCALE:
@@ -261,7 +256,6 @@ namespace OpenSim.Region.Physics.OdePlugin
// m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
break;
}
}//end ProcessVectorVehicleParam
internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
@@ -272,12 +266,11 @@ namespace OpenSim.Region.Physics.OdePlugin
// m_referenceFrame = pValue;
break;
}
}//end ProcessRotationVehicleParam
internal void ProcessTypeChange(Vehicle pType)
{
// Set Defaults For Type
// Set Defaults For Type
m_type = pType;
switch (pType)
{
@@ -357,8 +350,8 @@ namespace OpenSim.Region.Physics.OdePlugin
// m_bankingMix = 0.8f;
// m_bankingTimescale = 1;
// m_referenceFrame = Quaternion.Identity;
m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY |
VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY |
VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY |
VehicleFlag.LIMIT_MOTOR_UP);
break;
@@ -432,24 +425,25 @@ namespace OpenSim.Region.Physics.OdePlugin
{
if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
return;
frcount++; // used to limit debug comment output
frcount++; // used to limit debug comment output
if (frcount > 100)
frcount = 0;
MoveLinear(pTimestep, pParentScene);
MoveLinear(pTimestep, pParentScene);
MoveAngular(pTimestep);
}// end Step
private void MoveLinear(float pTimestep, OdeScene _pParentScene)
{
if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant
if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant
{
if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
if (!d.BodyIsEnabled(Body))
d.BodyEnable(Body);
// add drive to body
Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep);
m_lastLinearVelocityVector += (addAmount*10); // lastLinearVelocityVector is the current body velocity vector?
m_lastLinearVelocityVector += (addAmount*10); // lastLinearVelocityVector is the current body velocity vector?
// This will work temporarily, but we really need to compare speed on an axis
// KF: Limit body velocity to applied velocity?
if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X))
@@ -458,7 +452,7 @@ namespace OpenSim.Region.Physics.OdePlugin
m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y;
if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z))
m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z;
// decay applied velocity
Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep)));
//Console.WriteLine("decay: " + decayfraction);
@@ -466,194 +460,192 @@ namespace OpenSim.Region.Physics.OdePlugin
//Console.WriteLine("actual: " + m_linearMotorDirection);
}
else
{ // requested is not significant
// if what remains of applied is small, zero it.
if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
m_lastLinearVelocityVector = Vector3.Zero;
}
// convert requested object velocity to world-referenced vector
{ // requested is not significant
// if what remains of applied is small, zero it.
if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
m_lastLinearVelocityVector = Vector3.Zero;
}
// convert requested object velocity to world-referenced vector
m_dir = m_lastLinearVelocityVector;
d.Quaternion rot = d.BodyGetQuaternion(Body);
Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
m_dir *= rotq; // apply obj rotation to velocity vector
Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
m_dir *= rotq; // apply obj rotation to velocity vector
// add Gravity andBuoyancy
// KF: So far I have found no good method to combine a script-requested
// .Z velocity and gravity. Therefore only 0g will used script-requested
// .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only.
// add Gravity andBuoyancy
// KF: So far I have found no good method to combine a script-requested
// .Z velocity and gravity. Therefore only 0g will used script-requested
// .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only.
Vector3 grav = Vector3.Zero;
if(m_VehicleBuoyancy < 1.0f)
{
// There is some gravity, make a gravity force vector
// that is applied after object velocity.
d.Mass objMass;
d.BodyGetMass(Body, out objMass);
// m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy);
// Preserve the current Z velocity
d.Vector3 vel_now = d.BodyGetLinearVel(Body);
m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity
} // else its 1.0, no gravity.
// Check if hovering
if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
{
// We should hover, get the target height
d.Vector3 pos = d.BodyGetPosition(Body);
if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
{
m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight;
}
else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
{
m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
}
else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
{
m_VhoverTargetHeight = m_VhoverHeight;
}
if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
{
// If body is aready heigher, use its height as target height
if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
}
// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
// m_VhoverTimescale = 0f; // time to acheive height
// pTimestep is time since last frame,in secs
float herr0 = pos.Z - m_VhoverTargetHeight;
// Replace Vertical speed with correction figure if significant
if(Math.Abs(herr0) > 0.01f )
{
d.Mass objMass;
d.BodyGetMass(Body, out objMass);
m_dir.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
//KF: m_VhoverEfficiency is not yet implemented
}
else
{
m_dir.Z = 0f;
}
}
// Apply velocity
d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z);
if (m_VehicleBuoyancy < 1.0f)
{
// There is some gravity, make a gravity force vector
// that is applied after object velocity.
d.Mass objMass;
d.BodyGetMass(Body, out objMass);
// m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy);
// Preserve the current Z velocity
d.Vector3 vel_now = d.BodyGetLinearVel(Body);
m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity
} // else its 1.0, no gravity.
// Check if hovering
if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
{
// We should hover, get the target height
d.Vector3 pos = d.BodyGetPosition(Body);
if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
{
m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight;
}
else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
{
m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
}
else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
{
m_VhoverTargetHeight = m_VhoverHeight;
}
if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
{
// If body is aready heigher, use its height as target height
if (pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
}
// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
// m_VhoverTimescale = 0f; // time to acheive height
// pTimestep is time since last frame,in secs
float herr0 = pos.Z - m_VhoverTargetHeight;
// Replace Vertical speed with correction figure if significant
if (Math.Abs(herr0) > 0.01f)
{
d.Mass objMass;
d.BodyGetMass(Body, out objMass);
m_dir.Z = - ((herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
//KF: m_VhoverEfficiency is not yet implemented
}
else
{
m_dir.Z = 0f;
}
}
// Apply velocity
d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z);
// apply gravity force
d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
// apply friction
// apply friction
Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
} // end MoveLinear()
private void MoveAngular(float pTimestep)
{
/*
private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
private int m_angularMotorApply = 0; // application frame counter
private float m_angularMotorVelocity = 0; // current angular motor velocity (ramps up and down)
private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate
private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate
private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
*/
// Get what the body is doing, this includes 'external' influences
d.Vector3 angularVelocity = d.BodyGetAngularVel(Body);
// Vector3 angularVelocity = Vector3.Zero;
if (m_angularMotorApply > 0)
{
// ramp up to new value
// current velocity += error / ( time to get there / step interval )
// requested speed - last motor speed
m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep);
m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep);
m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep);
/*
private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
private int m_angularMotorApply = 0; // application frame counter
private float m_angularMotorVelocity = 0; // current angular motor velocity (ramps up and down)
private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate
private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate
private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
*/
m_angularMotorApply--; // This is done so that if script request rate is less than phys frame rate the expected
// velocity may still be acheived.
}
else
{
// no motor recently applied, keep the body velocity
/* m_angularMotorVelocity.X = angularVelocity.X;
m_angularMotorVelocity.Y = angularVelocity.Y;
m_angularMotorVelocity.Z = angularVelocity.Z; */
// and decay the velocity
m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep);
} // end motor section
// Get what the body is doing, this includes 'external' influences
d.Vector3 angularVelocity = d.BodyGetAngularVel(Body);
// Vector3 angularVelocity = Vector3.Zero;
if (m_angularMotorApply > 0)
{
// ramp up to new value
// current velocity += error / (time to get there / step interval)
// requested speed - last motor speed
m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep);
m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep);
m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep);
m_angularMotorApply--; // This is done so that if script request rate is less than phys frame rate the expected
// velocity may still be acheived.
}
else
{
// no motor recently applied, keep the body velocity
/* m_angularMotorVelocity.X = angularVelocity.X;
m_angularMotorVelocity.Y = angularVelocity.Y;
m_angularMotorVelocity.Z = angularVelocity.Z; */
// and decay the velocity
m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep);
} // end motor section
// Vertical attractor section
Vector3 vertattr = Vector3.Zero;
if(m_verticalAttractionTimescale < 300)
{
float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
// get present body rotation
d.Quaternion rot = d.BodyGetQuaternion(Body);
Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
// make a vector pointing up
Vector3 verterr = Vector3.Zero;
verterr.Z = 1.0f;
// rotate it to Body Angle
verterr = verterr * rotq;
// 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.
// 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
// negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
if (verterr.Z < 0.0f)
{
verterr.X = 2.0f - verterr.X;
verterr.Y = 2.0f - verterr.Y;
}
// Error is 0 (no error) to +/- 2 (max error)
// scale it by VAservo
verterr = verterr * VAservo;
//if(frcount == 0) Console.WriteLine("VAerr=" + verterr);
Vector3 vertattr = Vector3.Zero;
// As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
// Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
vertattr.X = verterr.Y;
vertattr.Y = - verterr.X;
vertattr.Z = 0f;
// scaling appears better usingsquare-law
float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
vertattr.X += bounce * angularVelocity.X;
vertattr.Y += bounce * angularVelocity.Y;
} // else vertical attractor is off
// m_lastVertAttractor = vertattr;
// Bank section tba
// Deflection section tba
// Sum velocities
m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection
if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
if (m_verticalAttractionTimescale < 300)
{
if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
}
else
{
m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
}
// apply friction
float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
// get present body rotation
d.Quaternion rot = d.BodyGetQuaternion(Body);
Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
// make a vector pointing up
Vector3 verterr = Vector3.Zero;
verterr.Z = 1.0f;
// rotate it to Body Angle
verterr = verterr * rotq;
// 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.
// 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
// negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
if (verterr.Z < 0.0f)
{
verterr.X = 2.0f - verterr.X;
verterr.Y = 2.0f - verterr.Y;
}
// Error is 0 (no error) to +/- 2 (max error)
// scale it by VAservo
verterr = verterr * VAservo;
//if (frcount == 0) Console.WriteLine("VAerr=" + verterr);
// As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
// Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
vertattr.X = verterr.Y;
vertattr.Y = - verterr.X;
vertattr.Z = 0f;
// scaling appears better usingsquare-law
float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
vertattr.X += bounce * angularVelocity.X;
vertattr.Y += bounce * angularVelocity.Y;
} // else vertical attractor is off
// m_lastVertAttractor = vertattr;
// Bank section tba
// Deflection section tba
// Sum velocities
m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection
if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
{
if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
}
else
{
m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
}
// apply friction
Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;
// Apply to the body
d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
} //end MoveAngular
}
m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;
// Apply to the body
d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
} //end MoveAngular
}
}

498
OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
View File

@@ -1,5 +1,7 @@
/* Copyright (c) Contributors, http://opensimulator.org/
/*
* Copyright (c) Contributors, http://opensimulator.org/
* See CONTRIBUTORS.TXT for a full list of copyright holders.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright
@@ -93,7 +95,7 @@ namespace OpenSim.Region.Physics.OdePlugin
private float m_targetHoverHeight;
private float m_groundHeight;
private float m_waterHeight;
private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
// private float m_tensor = 5f;
private int body_autodisable_frames = 20;
@@ -294,7 +296,7 @@ namespace OpenSim.Region.Physics.OdePlugin
m_taintselected = value;
m_isSelected = value;
}
if(m_isSelected) disableBodySoft();
if (m_isSelected) disableBodySoft();
}
}
@@ -302,7 +304,7 @@ namespace OpenSim.Region.Physics.OdePlugin
{
prev_geom = prim_geom;
prim_geom = geom;
//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName);
//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName);
if (prim_geom != IntPtr.Zero)
{
d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
@@ -314,7 +316,7 @@ namespace OpenSim.Region.Physics.OdePlugin
if (_parent != null && _parent is OdePrim)
{
OdePrim parent = (OdePrim)_parent;
//Console.WriteLine("SetGeom calls ChildSetGeom");
//Console.WriteLine("SetGeom calls ChildSetGeom");
parent.ChildSetGeom(this);
}
}
@@ -331,7 +333,7 @@ namespace OpenSim.Region.Physics.OdePlugin
{
d.BodyEnable(Body);
if (m_vehicle.Type != Vehicle.TYPE_NONE)
m_vehicle.Enable(Body, _parent_scene);
m_vehicle.Enable(Body, _parent_scene);
}
m_disabled = false;
@@ -376,7 +378,7 @@ namespace OpenSim.Region.Physics.OdePlugin
d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
// disconnect from world gravity so we can apply buoyancy
d.BodySetGravityMode (Body, false);
d.BodySetGravityMode (Body, false);
m_interpenetrationcount = 0;
m_collisionscore = 0;
@@ -872,7 +874,7 @@ namespace OpenSim.Region.Physics.OdePlugin
public void ProcessTaints(float timestep)
{
//Console.WriteLine("ProcessTaints for " + m_primName );
//Console.WriteLine("ProcessTaints for " + m_primName);
if (m_taintadd)
{
changeadd(timestep);
@@ -880,24 +882,24 @@ namespace OpenSim.Region.Physics.OdePlugin
if (prim_geom != IntPtr.Zero)
{
if (!_position.ApproxEquals(m_taintposition, 0f))
changemove(timestep);
if (!_position.ApproxEquals(m_taintposition, 0f))
changemove(timestep);
if (m_taintrot != _orientation)
{
if(childPrim && IsPhysical) // For physical child prim...
{
rotate(timestep);
// KF: ODE will also rotate the parent prim!
// so rotate the root back to where it was
OdePrim parent = (OdePrim)_parent;
parent.rotate(timestep);
if (m_taintrot != _orientation)
{
if (childPrim && IsPhysical) // For physical child prim...
{
rotate(timestep);
// KF: ODE will also rotate the parent prim!
// so rotate the root back to where it was
OdePrim parent = (OdePrim)_parent;
parent.rotate(timestep);
}
else
{
//Just rotate the prim
rotate(timestep);
}
//Just rotate the prim
rotate(timestep);
}
}
//
@@ -1006,7 +1008,7 @@ namespace OpenSim.Region.Physics.OdePlugin
// destroy link
else if (_parent != null && m_taintparent == null)
{
//Console.WriteLine(" changelink B");
//Console.WriteLine(" changelink B");
if (_parent is OdePrim)
{
@@ -1033,7 +1035,7 @@ namespace OpenSim.Region.Physics.OdePlugin
// prim is the child
public void ParentPrim(OdePrim prim)
{
//Console.WriteLine("ParentPrim " + m_primName);
//Console.WriteLine("ParentPrim " + m_primName);
if (this.m_localID != prim.m_localID)
{
if (Body == IntPtr.Zero)
@@ -1047,7 +1049,7 @@ namespace OpenSim.Region.Physics.OdePlugin
{
if (!childrenPrim.Contains(prim))
{
//Console.WriteLine("childrenPrim.Add " + prim);
//Console.WriteLine("childrenPrim.Add " + prim);
childrenPrim.Add(prim);
foreach (OdePrim prm in childrenPrim)
@@ -1080,7 +1082,7 @@ namespace OpenSim.Region.Physics.OdePlugin
m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet");
continue;
}
//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + m_primName);
//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + m_primName);
d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
@@ -1128,7 +1130,7 @@ namespace OpenSim.Region.Physics.OdePlugin
m_collisionCategories |= CollisionCategories.Body;
m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + m_primName);
//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + m_primName);
d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
//Console.WriteLine(" Post GeomSetCategoryBits 2");
d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
@@ -1203,7 +1205,7 @@ namespace OpenSim.Region.Physics.OdePlugin
{
foreach (OdePrim prm in childrenPrim)
{
//Console.WriteLine("ChildSetGeom calls ParentPrim");
//Console.WriteLine("ChildSetGeom calls ParentPrim");
ParentPrim(prm);
}
}
@@ -1230,7 +1232,7 @@ namespace OpenSim.Region.Physics.OdePlugin
lock (childrenPrim)
{
//Console.WriteLine("childrenPrim.Remove " + odePrim);
//Console.WriteLine("childrenPrim.Remove " + odePrim);
childrenPrim.Remove(odePrim);
}
@@ -1248,7 +1250,7 @@ namespace OpenSim.Region.Physics.OdePlugin
{
foreach (OdePrim prm in childrenPrim)
{
//Console.WriteLine("ChildDelink calls ParentPrim");
//Console.WriteLine("ChildDelink calls ParentPrim");
ParentPrim(prm);
}
}
@@ -1350,7 +1352,7 @@ namespace OpenSim.Region.Physics.OdePlugin
public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh)
{
//Console.WriteLine("CreateGeom:");
//Console.WriteLine("CreateGeom:");
if (_mesh != null)
{
setMesh(_parent_scene, _mesh);
@@ -1381,7 +1383,7 @@ namespace OpenSim.Region.Physics.OdePlugin
_parent_scene.waitForSpaceUnlock(m_targetSpace);
try
{
//Console.WriteLine(" CreateGeom 2");
//Console.WriteLine(" CreateGeom 2");
SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
}
catch (AccessViolationException)
@@ -1397,7 +1399,7 @@ namespace OpenSim.Region.Physics.OdePlugin
_parent_scene.waitForSpaceUnlock(m_targetSpace);
try
{
//Console.WriteLine(" CreateGeom 3");
//Console.WriteLine(" CreateGeom 3");
SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
}
catch (AccessViolationException)
@@ -1414,7 +1416,7 @@ namespace OpenSim.Region.Physics.OdePlugin
_parent_scene.waitForSpaceUnlock(m_targetSpace);
try
{
//Console.WriteLine(" CreateGeom 4");
//Console.WriteLine(" CreateGeom 4");
SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
}
catch (AccessViolationException)
@@ -1451,7 +1453,7 @@ namespace OpenSim.Region.Physics.OdePlugin
lock (_parent_scene.OdeLock)
{
//Console.WriteLine("changeadd 1");
//Console.WriteLine("changeadd 1");
CreateGeom(m_targetSpace, _mesh);
if (prim_geom != IntPtr.Zero)
@@ -1508,7 +1510,7 @@ namespace OpenSim.Region.Physics.OdePlugin
if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
{
// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
Console.WriteLine(" JointCreateFixed");
Console.WriteLine(" JointCreateFixed");
m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
d.JointAttach(m_linkJoint, Body, odParent.Body);
d.JointSetFixed(m_linkJoint);
@@ -1562,244 +1564,244 @@ Console.WriteLine(" JointCreateFixed");
float fz = 0;
if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
{
if (m_vehicle.Type != Vehicle.TYPE_NONE)
{
// 'VEHICLES' are dealt with in ODEDynamics.cs
m_vehicle.Step(timestep, _parent_scene);
}
else
{
//Console.WriteLine("Move " + m_primName);
if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
// NON-'VEHICLES' are dealt with here
if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f))
{
d.Vector3 avel2 = d.BodyGetAngularVel(Body);
if (m_angularlock.X == 1)
avel2.X = 0;
if (m_angularlock.Y == 1)
avel2.Y = 0;
if (m_angularlock.Z == 1)
avel2.Z = 0;
d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
}
//float PID_P = 900.0f;
if (m_vehicle.Type != Vehicle.TYPE_NONE)
{
// 'VEHICLES' are dealt with in ODEDynamics.cs
m_vehicle.Step(timestep, _parent_scene);
}
else
{
//Console.WriteLine("Move " + m_primName);
if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
// NON-'VEHICLES' are dealt with here
if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f))
{
d.Vector3 avel2 = d.BodyGetAngularVel(Body);
if (m_angularlock.X == 1)
avel2.X = 0;
if (m_angularlock.Y == 1)
avel2.Y = 0;
if (m_angularlock.Z == 1)
avel2.Z = 0;
d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
}
//float PID_P = 900.0f;
float m_mass = CalculateMass();
float m_mass = CalculateMass();
// fz = 0f;
// fz = 0f;
//m_log.Info(m_collisionFlags.ToString());
//KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
// would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ??
// m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
// gravityz multiplier = 1 - m_buoyancy
fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass;
//KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
// would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ??
// m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
// gravityz multiplier = 1 - m_buoyancy
fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass;
if (m_usePID)
{
//Console.WriteLine("PID " + m_primName);
// KF - this is for object move? eg. llSetPos() ?
//if (!d.BodyIsEnabled(Body))
//d.BodySetForce(Body, 0f, 0f, 0f);
// If we're using the PID controller, then we have no gravity
//fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply...
fz = 0f;
if (m_usePID)
{
//Console.WriteLine("PID " + m_primName);
// KF - this is for object move? eg. llSetPos() ?
//if (!d.BodyIsEnabled(Body))
//d.BodySetForce(Body, 0f, 0f, 0f);
// If we're using the PID controller, then we have no gravity
//fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply...
fz = 0f;
// no lock; for now it's only called from within Simulate()
// If the PID Controller isn't active then we set our force
// calculating base velocity to the current position
// no lock; for now it's only called from within Simulate()
// If the PID Controller isn't active then we set our force
// calculating base velocity to the current position
if ((m_PIDTau < 1) && (m_PIDTau != 0))
{
//PID_G = PID_G / m_PIDTau;
m_PIDTau = 1;
}
if ((PID_G - m_PIDTau) <= 0)
{
PID_G = m_PIDTau + 1;
}
//PidStatus = true;
if ((m_PIDTau < 1) && (m_PIDTau != 0))
{
//PID_G = PID_G / m_PIDTau;
m_PIDTau = 1;
}
if ((PID_G - m_PIDTau) <= 0)
{
PID_G = m_PIDTau + 1;
}
//PidStatus = true;
// PhysicsVector vec = new PhysicsVector();
d.Vector3 vel = d.BodyGetLinearVel(Body);
// PhysicsVector vec = new PhysicsVector();
d.Vector3 vel = d.BodyGetLinearVel(Body);
d.Vector3 pos = d.BodyGetPosition(Body);
_target_velocity =
d.Vector3 pos = d.BodyGetPosition(Body);
_target_velocity =
new Vector3(
(m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
(m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
(m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
);
(m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
(m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
(m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
);
// if velocity is zero, use position control; otherwise, velocity control
// if velocity is zero, use position control; otherwise, velocity control
if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
{
// keep track of where we stopped. No more slippin' & slidin'
// We only want to deactivate the PID Controller if we think we want to have our surrogate
// react to the physics scene by moving it's position.
// Avatar to Avatar collisions
// Prim to avatar collisions
if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
{
// keep track of where we stopped. No more slippin' & slidin'
// We only want to deactivate the PID Controller if we think we want to have our surrogate
// react to the physics scene by moving it's position.
// Avatar to Avatar collisions
// Prim to avatar collisions
//fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
//fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
//fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
d.BodySetLinearVel(Body, 0, 0, 0);
d.BodyAddForce(Body, 0, 0, fz);
return;
}
else
{
_zeroFlag = false;
//fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
//fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
//fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
d.BodySetLinearVel(Body, 0, 0, 0);
d.BodyAddForce(Body, 0, 0, fz);
return;
}
else
{
_zeroFlag = false;
// We're flying and colliding with something
fx = ((_target_velocity.X) - vel.X) * (PID_D);
fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
// vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
// We're flying and colliding with something
fx = ((_target_velocity.X) - vel.X) * (PID_D);
fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
// vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
}
} // end if (m_usePID)
fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
}
} // end if (m_usePID)
// Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
if (m_useHoverPID && !m_usePID)
{
//Console.WriteLine("Hover " + m_primName);
// If we're using the PID controller, then we have no gravity
fz = (-1 * _parent_scene.gravityz) * m_mass;
// Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
if (m_useHoverPID && !m_usePID)
{
//Console.WriteLine("Hover " + m_primName);
// If we're using the PID controller, then we have no gravity
fz = (-1 * _parent_scene.gravityz) * m_mass;
// no lock; for now it's only called from within Simulate()
// no lock; for now it's only called from within Simulate()
// If the PID Controller isn't active then we set our force
// calculating base velocity to the current position
// If the PID Controller isn't active then we set our force
// calculating base velocity to the current position
if ((m_PIDTau < 1))
{
PID_G = PID_G / m_PIDTau;
}
if ((m_PIDTau < 1))
{
PID_G = PID_G / m_PIDTau;
}
if ((PID_G - m_PIDTau) <= 0)
{
PID_G = m_PIDTau + 1;
}
if ((PID_G - m_PIDTau) <= 0)
{
PID_G = m_PIDTau + 1;
}
// Where are we, and where are we headed?
d.Vector3 pos = d.BodyGetPosition(Body);
d.Vector3 vel = d.BodyGetLinearVel(Body);
// Where are we, and where are we headed?
d.Vector3 pos = d.BodyGetPosition(Body);
d.Vector3 vel = d.BodyGetLinearVel(Body);
// Non-Vehicles have a limited set of Hover options.
// determine what our target height really is based on HoverType
switch (m_PIDHoverType)
{
case PIDHoverType.Ground:
m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
break;
case PIDHoverType.GroundAndWater:
m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
m_waterHeight = _parent_scene.GetWaterLevel();
if (m_groundHeight > m_waterHeight)
{
m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
}
else
{
m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
}
break;
// Non-Vehicles have a limited set of Hover options.
// determine what our target height really is based on HoverType
switch (m_PIDHoverType)
{
case PIDHoverType.Ground:
m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
break;
case PIDHoverType.GroundAndWater:
m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
m_waterHeight = _parent_scene.GetWaterLevel();
if (m_groundHeight > m_waterHeight)
{
m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
}
else
{
m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
}
break;
} // end switch (m_PIDHoverType)
} // end switch (m_PIDHoverType)
_target_velocity =
_target_velocity =
new Vector3(0.0f, 0.0f,
(m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
);
(m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
);
// if velocity is zero, use position control; otherwise, velocity control
// if velocity is zero, use position control; otherwise, velocity control
if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
{
// keep track of where we stopped. No more slippin' & slidin'
// We only want to deactivate the PID Controller if we think we want to have our surrogate
// react to the physics scene by moving it's position.
// Avatar to Avatar collisions
// Prim to avatar collisions
if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
{
// keep track of where we stopped. No more slippin' & slidin'
// We only want to deactivate the PID Controller if we think we want to have our surrogate
// react to the physics scene by moving it's position.
// Avatar to Avatar collisions
// Prim to avatar collisions
d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
d.BodyAddForce(Body, 0, 0, fz);
return;
}
else
{
_zeroFlag = false;
d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
d.BodyAddForce(Body, 0, 0, fz);
return;
}
else
{
_zeroFlag = false;
// We're flying and colliding with something
fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
}
}
// We're flying and colliding with something
fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
}
}
fx *= m_mass;
fy *= m_mass;
//fz *= m_mass;
fx *= m_mass;
fy *= m_mass;
//fz *= m_mass;
fx += m_force.X;
fy += m_force.Y;
fz += m_force.Z;
fx += m_force.X;
fy += m_force.Y;
fz += m_force.Z;
//m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
if (fx != 0 || fy != 0 || fz != 0)
{
//m_taintdisable = true;
//base.RaiseOutOfBounds(Position);
//d.BodySetLinearVel(Body, fx, fy, 0f);
if (!d.BodyIsEnabled(Body))
{
// A physical body at rest on a surface will auto-disable after a while,
// this appears to re-enable it incase the surface it is upon vanishes,
// and the body should fall again.
d.BodySetLinearVel(Body, 0f, 0f, 0f);
d.BodySetForce(Body, 0, 0, 0);
enableBodySoft();
}
//m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
if (fx != 0 || fy != 0 || fz != 0)
{
//m_taintdisable = true;
//base.RaiseOutOfBounds(Position);
//d.BodySetLinearVel(Body, fx, fy, 0f);
if (!d.BodyIsEnabled(Body))
{
// A physical body at rest on a surface will auto-disable after a while,
// this appears to re-enable it incase the surface it is upon vanishes,
// and the body should fall again.
d.BodySetLinearVel(Body, 0f, 0f, 0f);
d.BodySetForce(Body, 0, 0, 0);
enableBodySoft();
}
// 35x10 = 350n times the mass per second applied maximum.
float nmax = 35f * m_mass;
float nmin = -35f * m_mass;
// 35x10 = 350n times the mass per second applied maximum.
float nmax = 35f * m_mass;
float nmin = -35f * m_mass;
if (fx > nmax)
fx = nmax;
if (fx < nmin)
fx = nmin;
if (fy > nmax)
fy = nmax;
if (fy < nmin)
fy = nmin;
d.BodyAddForce(Body, fx, fy, fz);
//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
}
}
if (fx > nmax)
fx = nmax;
if (fx < nmin)
fx = nmin;
if (fy > nmax)
fy = nmax;
if (fy < nmin)
fy = nmin;
d.BodyAddForce(Body, fx, fy, fz);
//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
}
}
}
else
{ // is not physical, or is not a body or is selected
{ // is not physical, or is not a body or is selected
// _zeroPosition = d.BodyGetPosition(Body);
return;
//Console.WriteLine("Nothing " + m_primName);
//Console.WriteLine("Nothing " + m_primName);
}
}
@@ -1815,18 +1817,18 @@ Console.WriteLine(" JointCreateFixed");
myrot.W = _orientation.W;
if (Body != IntPtr.Zero)
{
// KF: If this is a root prim do BodySet
// KF: If this is a root prim do BodySet
d.BodySetQuaternion(Body, ref myrot);
if (m_isphysical)
{
if (m_isphysical)
{
if (!m_angularlock.ApproxEquals(Vector3.One, 0f))
createAMotor(m_angularlock);
}
}
else
{
// daughter prim, do Geom set
d.GeomSetQuaternion(prim_geom, ref myrot);
createAMotor(m_angularlock);
}
}
else
{
// daughter prim, do Geom set
d.GeomSetQuaternion(prim_geom, ref myrot);
}
resetCollisionAccounting();
@@ -1890,7 +1892,7 @@ Console.WriteLine(" JointCreateFixed");
m_log.Error("[PHYSICS]: PrimGeom dead");
}
}
//Console.WriteLine("changePhysicsStatus for " + m_primName );
//Console.WriteLine("changePhysicsStatus for " + m_primName);
changeadd(2f);
}
if (childPrim)
@@ -1976,7 +1978,7 @@ Console.WriteLine(" JointCreateFixed");
else
{
_mesh = null;
//Console.WriteLine("changesize 2");
//Console.WriteLine("changesize 2");
CreateGeom(m_targetSpace, _mesh);
}
@@ -2083,7 +2085,7 @@ Console.WriteLine(" JointCreateFixed");
else
{
_mesh = null;
//Console.WriteLine("changeshape");
//Console.WriteLine("changeshape");
CreateGeom(m_targetSpace, null);
}
@@ -2454,7 +2456,7 @@ Console.WriteLine(" JointCreateFixed");
if (QuaternionIsFinite(value))
{
_orientation = value;
}
}
else
m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object");
@@ -2675,8 +2677,8 @@ Console.WriteLine(" JointCreateFixed");
//outofBounds = true;
}
//float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation));
//Console.WriteLine("Adiff " + m_primName + " = " + Adiff);
//float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation));
//Console.WriteLine("Adiff " + m_primName + " = " + Adiff);
if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
&& (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
&& (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
@@ -2684,7 +2686,7 @@ Console.WriteLine(" JointCreateFixed");
&& (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large
{
_zeroFlag = true;
//Console.WriteLine("ZFT 2");
//Console.WriteLine("ZFT 2");
m_throttleUpdates = false;
}
else

2
OpenSim/Region/Physics/OdePlugin/ODERayCastRequestManager.cs
View File

@@ -110,7 +110,7 @@ namespace OpenSim.Region.Physics.OdePlugin
for (int i = 0; i < reqs.Length; i++)
{
if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast
RayCast(reqs[i]); // if there isn't anyone to send results
RayCast(reqs[i]); // if there isn't anyone to send results
}
/*
foreach (ODERayCastRequest req in m_PendingRequests)

20
OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
View File

@@ -2152,7 +2152,7 @@ namespace OpenSim.Region.Physics.OdePlugin
/// <param name="prim"></param>
public void RemovePrimThreadLocked(OdePrim prim)
{
//Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName);
//Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName);
lock (prim)
{
remCollisionEventReporting(prim);
@@ -2603,12 +2603,12 @@ namespace OpenSim.Region.Physics.OdePlugin
lock (_taintedPrimLock)
{
if (!(_taintedPrimH.Contains(taintedprim)))
{
//Console.WriteLine("AddPhysicsActorTaint to " + taintedprim.m_primName);
_taintedPrimH.Add(taintedprim); // HashSet for searching
_taintedPrimL.Add(taintedprim); // List for ordered readout
}
}
{
//Console.WriteLine("AddPhysicsActorTaint to " + taintedprim.m_primName);
_taintedPrimH.Add(taintedprim); // HashSet for searching
_taintedPrimL.Add(taintedprim); // List for ordered readout
}
}
return;
}
else if (prim is OdeCharacter)
@@ -2736,12 +2736,12 @@ namespace OpenSim.Region.Physics.OdePlugin
{
if (prim.m_taintremove)
{
//Console.WriteLine("Simulate calls RemovePrimThreadLocked");
//Console.WriteLine("Simulate calls RemovePrimThreadLocked");
RemovePrimThreadLocked(prim);
}
else
{
//Console.WriteLine("Simulate calls ProcessTaints");
//Console.WriteLine("Simulate calls ProcessTaints");
prim.ProcessTaints(timeStep);
}
processedtaints = true;
@@ -2937,7 +2937,7 @@ namespace OpenSim.Region.Physics.OdePlugin
}
if (processedtaints)
//Console.WriteLine("Simulate calls Clear of _taintedPrim list");
//Console.WriteLine("Simulate calls Clear of _taintedPrim list");
_taintedPrimH.Clear();
_taintedPrimL.Clear();
}