OpenSim
/
OpenSimMirror
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

BulletSim: Add very detailed logging to BSDynamics for vehicle debugging

0.7.4.1
Robert Adams 2012-07-18 08:36:41 -07:00
parent e9c437ed0e
commit cda67a68de
3 changed files with 146 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

130
OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
View File

@ -131,8 +131,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin
m_type = Vehicle.TYPE_NONE;
}
internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue, float timestep)
{
DetailLog("{0},ProcessFloatVehicleParam,param={1},val={2}", m_prim.LocalID, pParam, pValue);
switch (pParam)
{
case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
@ -229,8 +230,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin
}
}//end ProcessFloatVehicleParam
internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue, float timestep)
{
DetailLog("{0},ProcessVectorVehicleParam,param={1},val={2}", m_prim.LocalID, pParam, pValue);
switch (pParam)
{
case Vehicle.ANGULAR_FRICTION_TIMESCALE:
@ -265,6 +267,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
{
DetailLog("{0},ProcessRotationalVehicleParam,param={1},val={2}", m_prim.LocalID, pParam, pValue);
switch (pParam)
{
case Vehicle.REFERENCE_FRAME:
@ -278,6 +281,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
internal void ProcessVehicleFlags(int pParam, bool remove)
{
DetailLog("{0},ProcessVehicleFlags,param={1},remove={2}", m_prim.LocalID, pParam, remove);
if (remove)
{
if (pParam == -1)
@ -434,6 +438,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
internal void ProcessTypeChange(Vehicle pType)
{
DetailLog("{0},ProcessTypeChange,type={1}", m_prim.LocalID, pType);
// Set Defaults For Type
m_type = pType;
switch (pType)
@ -594,7 +599,6 @@ namespace OpenSim.Region.Physics.BulletSPlugin
m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
m_Hoverflags |= (VehicleFlag.HOVER_GLOBAL_HEIGHT);
break;
}
}//end SetDefaultsForType
@ -609,12 +613,17 @@ namespace OpenSim.Region.Physics.BulletSPlugin
MoveLinear(pTimestep, pParentScene);
MoveAngular(pTimestep);
LimitRotation(pTimestep);
DetailLog("{0},step,pos={1},force={2},velocity={3},angvel={4}",
m_prim.LocalID, m_prim.Position, m_prim.Force, m_prim.Velocity, m_prim.RotationalVelocity);
}// end Step
private void MoveLinear(float pTimestep, BSScene _pParentScene)
{
if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant
{
Vector3 origDir = m_linearMotorDirection;
Vector3 origVel = m_lastLinearVelocityVector;
// add drive to body
Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep);
m_lastLinearVelocityVector += (addAmount*10); // lastLinearVelocityVector is the current body velocity vector?
@ -630,9 +639,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
// decay applied velocity
Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep)));
//Console.WriteLine("decay: " + decayfraction);
m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f;
//Console.WriteLine("actual: " + m_linearMotorDirection);
DetailLog("{0},MoveLinear,nonZero,origdir={1},origvel={2},add={3},decay={4},dir={5},vel={6}",
m_prim.LocalID, origDir, origVel, addAmount, decayfraction, m_linearMotorDirection, m_lastLinearVelocityVector);
}
else
{ // requested is not significant
@ -643,63 +653,66 @@ namespace OpenSim.Region.Physics.BulletSPlugin
// convert requested object velocity to world-referenced vector
m_dir = m_lastLinearVelocityVector;
Quaternion rot = m_prim.Orientation;
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
m_dir *= m_prim.Orientation;
// add Gravity andBuoyancy
// Add the various forces into m_dir which will be our new direction vector (velocity)
// add Gravity and Buoyancy
// 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;
// There is some gravity, make a gravity force vector
// that is applied after object velocity.
float objMass = m_prim.Mass;
// There is some gravity, make a gravity force vector that is applied after object velocity.
// m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
grav.Z = _pParentScene.DefaultGravity.Z * objMass * (1f - m_VehicleBuoyancy);
grav.Z = _pParentScene.DefaultGravity.Z * m_prim.Mass * (1f - m_VehicleBuoyancy);
// Preserve the current Z velocity
Vector3 vel_now = m_prim.Velocity;
m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity
Vector3 pos = m_prim.Position;
Vector3 posChange = pos;
// Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f);
Vector3 posChange = new Vector3();
posChange.X = pos.X - m_lastPositionVector.X;
posChange.Y = pos.Y - m_lastPositionVector.Y;
posChange.Z = pos.Z - m_lastPositionVector.Z;
double Zchange = Math.Abs(posChange.Z);
if (m_BlockingEndPoint != Vector3.Zero)
{
bool changed = false;
if (pos.X >= (m_BlockingEndPoint.X - (float)1))
{
pos.X -= posChange.X + 1;
m_prim.Position = pos;
changed = true;
}
if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
{
pos.Y -= posChange.Y + 1;
m_prim.Position = pos;
changed = true;
}
if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
{
pos.Z -= posChange.Z + 1;
m_prim.Position = pos;
changed = true;
}
if (pos.X <= 0)
{
pos.X += posChange.X + 1;
m_prim.Position = pos;
changed = true;
}
if (pos.Y <= 0)
{
pos.Y += posChange.Y + 1;
changed = true;
}
if (changed)
{
m_prim.Position = pos;
DetailLog("{0},MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
m_prim.LocalID, m_BlockingEndPoint, posChange, pos);
}
}
if (pos.Z < _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y))
{
pos.Z = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + 2;
m_prim.Position = pos;
DetailLog("{0},MoveLinear,terrainHeight,pos={1}", m_prim.LocalID, pos);
}
// Check if hovering
@ -746,6 +759,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
}
}
DetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}", m_prim.LocalID, pos, m_dir, m_VhoverHeight, m_VhoverTargetHeight);
// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
// m_VhoverTimescale = 0f; // time to acheive height
// pTimestep is time since last frame,in secs
@ -774,12 +789,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
{
grav.Z = (float)(grav.Z * 1.125);
}
float terraintemp = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
float terraintemp = _pParentScene.GetTerrainHeightAtXYZ(pos);
float postemp = (pos.Z - terraintemp);
if (postemp > 2.5f)
{
grav.Z = (float)(grav.Z * 1.037125);
}
DetailLog("{0},MoveLinear,limitMotorUp,grav={1}", m_prim.LocalID, grav);
//End Experimental Values
}
if ((m_flags & (VehicleFlag.NO_X)) != 0)
@ -803,29 +819,35 @@ namespace OpenSim.Region.Physics.BulletSPlugin
m_prim.Force = grav;
// apply friction
// Apply friction
Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
DetailLog("{0},MoveLinear,done,pos={1},vel={2},force={3},decay={4}",
m_prim.LocalID, m_lastPositionVector, m_dir, grav, 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
*/
// m_angularMotorDirection // angular velocity requested by LSL motor
// m_angularMotorApply // application frame counter
// m_angularMotorVelocity // current angular motor velocity (ramps up and down)
// m_angularMotorTimescale // motor angular velocity ramp up rate
// m_angularMotorDecayTimescale // motor angular velocity decay rate
// m_angularFrictionTimescale // body angular velocity decay rate
// m_lastAngularVelocity // what was last applied to body
// Get what the body is doing, this includes 'external' influences
Vector3 angularVelocity = m_prim.RotationalVelocity;
// Vector3 angularVelocity = Vector3.Zero;
if (m_angularMotorApply > 0)
{
// Rather than snapping the angular motor velocity from the old value to
// a newly set velocity, this routine steps the value from the previous
// value (m_angularMotorVelocity) to the requested value (m_angularMotorDirection).
// There are m_angularMotorApply steps.
Vector3 origAngularVelocity = m_angularMotorVelocity;
// ramp up to new value
// current velocity += error / (time to get there / step interval)
// requested speed - last motor speed
@ -833,23 +855,21 @@ namespace OpenSim.Region.Physics.BulletSPlugin
m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep);
m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep);
DetailLog("{0},MoveAngular,angularMotorApply,apply={1},origvel={2},dir={3},vel={4}",
m_prim.LocalID,m_angularMotorApply,origAngularVelocity, m_angularMotorDirection, m_angularMotorVelocity);
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; */
// No motor recently applied, keep the body velocity
// 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);
@ -871,7 +891,6 @@ namespace OpenSim.Region.Physics.BulletSPlugin
// 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.
@ -884,11 +903,15 @@ namespace OpenSim.Region.Physics.BulletSPlugin
vertattr.X += bounce * angularVelocity.X;
vertattr.Y += bounce * angularVelocity.Y;
DetailLog("{0},MoveAngular,verticalAttraction,verterr={1},bounce={2},vertattr={3}",
m_prim.LocalID, verterr, bounce, vertattr);
} // else vertical attractor is off
// m_lastVertAttractor = vertattr;
// m_lastVertAttractor = vertattr;
// Bank section tba
// Deflection section tba
// Sum velocities
@ -898,11 +921,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
{
m_lastAngularVelocity.X = 0;
m_lastAngularVelocity.Y = 0;
DetailLog("{0},MoveAngular,noDeflectionUp,lastAngular={1}", m_prim.LocalID, m_lastAngularVelocity);
}
if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
{
m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
DetailLog("{0},MoveAngular,zeroSmallValues,lastAngular={1}", m_prim.LocalID, m_lastAngularVelocity);
}
// apply friction
@ -912,10 +937,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
// Apply to the body
m_prim.RotationalVelocity = m_lastAngularVelocity;
DetailLog("{0},MoveAngular,done,decay={1},lastAngular={2}", m_prim.LocalID, decayamount, m_lastAngularVelocity);
} //end MoveAngular
} //end MoveAngular
internal void LimitRotation(float timestep)
{
Quaternion rotq = m_prim.Orientation; // rotq = rotation of object
Quaternion rotq = m_prim.Orientation;
Quaternion m_rot = rotq;
bool changed = false;
if (m_RollreferenceFrame != Quaternion.Identity)
@ -923,18 +951,22 @@ namespace OpenSim.Region.Physics.BulletSPlugin
if (rotq.X >= m_RollreferenceFrame.X)
{
m_rot.X = rotq.X - (m_RollreferenceFrame.X / 2);
changed = true;
}
if (rotq.Y >= m_RollreferenceFrame.Y)
{
m_rot.Y = rotq.Y - (m_RollreferenceFrame.Y / 2);
changed = true;
}
if (rotq.X <= -m_RollreferenceFrame.X)
{
m_rot.X = rotq.X + (m_RollreferenceFrame.X / 2);
changed = true;
}
if (rotq.Y <= -m_RollreferenceFrame.Y)
{
m_rot.Y = rotq.Y + (m_RollreferenceFrame.Y / 2);
changed = true;
}
changed = true;
}
@ -944,8 +976,22 @@ namespace OpenSim.Region.Physics.BulletSPlugin
m_rot.Y = 0;
changed = true;
}
if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0)
{
m_rot.X = 0;
m_rot.Y = 0;
changed = true;
}
if (changed)
m_prim.Orientation = m_rot;
DetailLog("{0},LimitRotation,done,changed={1},orig={2},new={3}", m_prim.LocalID, changed, rotq, m_rot);
}
// Invoke the detailed logger and output something if it's enabled.
private void DetailLog(string msg, params Object[] args)
{
m_prim.Scene.VehicleLogging.Write(msg, args);
}
}
}

1
OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
View File

@ -52,6 +52,7 @@ public sealed class BSPrim : PhysicsActor
private List<ConvexResult> _hulls;
private BSScene _scene;
public BSScene Scene { get { return _scene; } }
private String _avName;
private uint _localID = 0;

60
OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
View File

@ -29,12 +29,13 @@ using System.Collections.Generic;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading;
using OpenSim.Framework;
using OpenSim.Region.CoreModules.Framework.Statistics.Logging;
using OpenSim.Region.Framework;
using OpenSim.Region.Physics.Manager;
using Nini.Config;
using log4net;
using OpenSim.Framework;
using OpenSim.Region.Physics.Manager;
using OpenMetaverse;
using OpenSim.Region.Framework;
// TODOs for BulletSim (for BSScene, BSPrim, BSCharacter and BulletSim)
// Debug linkset
@ -158,6 +159,19 @@ public class BSScene : PhysicsScene, IPhysicsParameters
private BulletSimAPI.DebugLogCallback m_DebugLogCallbackHandle;
// Sometimes you just have to log everything.
public LogWriter PhysicsLogging;
private bool m_physicsLoggingEnabled;
private string m_physicsLoggingDir;
private string m_physicsLoggingPrefix;
private int m_physicsLoggingFileMinutes;
public LogWriter VehicleLogging;
private bool m_vehicleLoggingEnabled;
private string m_vehicleLoggingDir;
private string m_vehicleLoggingPrefix;
private int m_vehicleLoggingFileMinutes;
public BSScene(string identifier)
{
m_initialized = false;
@ -178,6 +192,26 @@ public class BSScene : PhysicsScene, IPhysicsParameters
m_updateArray = new EntityProperties[m_maxUpdatesPerFrame];
m_updateArrayPinnedHandle = GCHandle.Alloc(m_updateArray, GCHandleType.Pinned);
// Enable very detailed logging.
// By creating an empty logger when not logging, the log message invocation code
// can be left in and every call doesn't have to check for null.
if (m_physicsLoggingEnabled)
{
PhysicsLogging = new LogWriter(m_physicsLoggingDir, m_physicsLoggingPrefix, m_physicsLoggingFileMinutes);
}
else
{
PhysicsLogging = new LogWriter();
}
if (m_vehicleLoggingEnabled)
{
VehicleLogging = new LogWriter(m_vehicleLoggingDir, m_vehicleLoggingPrefix, m_vehicleLoggingFileMinutes);
}
else
{
VehicleLogging = new LogWriter();
}
// Get the version of the DLL
// TODO: this doesn't work yet. Something wrong with marshaling the returned string.
// BulletSimVersion = BulletSimAPI.GetVersion();
@ -321,6 +355,17 @@ public class BSScene : PhysicsScene, IPhysicsParameters
parms.shouldSplitSimulationIslands = ParamBoolean(pConfig, "ShouldSplitSimulationIslands", parms.shouldSplitSimulationIslands);
parms.shouldEnableFrictionCaching = ParamBoolean(pConfig, "ShouldEnableFrictionCaching", parms.shouldEnableFrictionCaching);
parms.numberOfSolverIterations = pConfig.GetFloat("NumberOfSolverIterations", parms.numberOfSolverIterations);
// Very detailed logging for physics debugging
m_physicsLoggingEnabled = pConfig.GetBoolean("PhysicsLoggingEnabled", false);
m_physicsLoggingDir = pConfig.GetString("PhysicsLoggingDir", ".");
m_physicsLoggingPrefix = pConfig.GetString("PhysicsLoggingPrefix", "physics-");
m_physicsLoggingFileMinutes = pConfig.GetInt("PhysicsLoggingFileMinutes", 5);
// Very detailed logging for vehicle debugging
m_vehicleLoggingEnabled = pConfig.GetBoolean("VehicleLoggingEnabled", false);
m_vehicleLoggingDir = pConfig.GetString("VehicleLoggingDir", ".");
m_vehicleLoggingPrefix = pConfig.GetString("VehicleLoggingPrefix", "vehicle-");
m_vehicleLoggingFileMinutes = pConfig.GetInt("VehicleLoggingFileMinutes", 5);
}
}
m_params[0] = parms;
@ -560,8 +605,17 @@ public class BSScene : PhysicsScene, IPhysicsParameters
});
}
// Someday we will have complex terrain with caves and tunnels
// For the moment, it's flat and convex
public float GetTerrainHeightAtXYZ(Vector3 loc)
{
return GetTerrainHeightAtXY(loc.X, loc.Y);
}
public float GetTerrainHeightAtXY(float tX, float tY)
{
if (tX < 0 || tX >= Constants.RegionSize || tY < 0 || tY >= Constants.RegionSize)
return 30;
return m_heightMap[((int)tX) * Constants.RegionSize + ((int)tY)];
}