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

Changes to vehicles code etc. Includes some debug aids to remove later.

avinationmerge
UbitUmarov 2012-02-10 22:43:51 +00:00
parent 6af01f6767
commit 7cf73cb92a
4 changed files with 147 additions and 245 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

270
OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
View File

@ -629,23 +629,19 @@ namespace OpenSim.Region.Physics.OdePlugin
Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame
Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in object frame
d.Vector3 dtorque = new d.Vector3();// actually angular aceleration until mult by Inertia in object frame
bool doathing = false;
d.Vector3 dtorque = new d.Vector3();
// linear motor
if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000)
{
tmpV = m_linearMotorDirection - curLocalVel; // velocity error
if (tmpV.LengthSquared() > 1e-6f)
{
tmpV = tmpV * (m_lmEfect / m_linearMotorTimescale); // error to correct in this timestep
tmpV *= m_lmEfect / m_linearMotorTimescale; // error to correct in this timestep
tmpV *= rotq; // to world
if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
tmpV.Z = 0;
if (m_linearMotorOffset.X != 0 && m_linearMotorOffset.Y != 0 && m_linearMotorOffset.Z != 0)
if (m_linearMotorOffset.X != 0 || m_linearMotorOffset.Y != 0 || m_linearMotorOffset.Z != 0)
{
// have offset, do it now
tmpV *= rootPrim.Mass;
@ -657,8 +653,7 @@ namespace OpenSim.Region.Physics.OdePlugin
force.Y += tmpV.Y;
force.Z += tmpV.Z;
}
}
m_lmEfect *= (1 - 1.0f / m_linearMotorDecayTimescale);
m_lmEfect *= (1.0f - 1.0f / m_linearMotorDecayTimescale);
}
else
m_lmEfect = 0;
@ -719,30 +714,35 @@ namespace OpenSim.Region.Physics.OdePlugin
if (m_linearDeflectionEfficiency > 0)
{
float len = curVel.Length();
Vector3 atAxis = refAtAxis;
atAxis *= rotq; // at axis rotated to world
atAxis = Xrot(rotq);
tmpV = atAxis * len;
tmpV -= curVel; // velocity error
Vector3 atAxis;
atAxis = Xrot(rotq); // where are we pointing to
atAxis *= len; // make it same size as world velocity vector
tmpV = -atAxis; // oposite direction
atAxis -= curVel; // error to one direction
len = atAxis.LengthSquared();
tmpV -= curVel; // error to oposite
float lens = tmpV.LengthSquared();
if (len > 0.01 || lens > 0.01) // do nothing if close enougth
{
if (len < lens)
tmpV = atAxis;
tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep
force.X += tmpV.X;
force.Y += tmpV.Y;
if((m_flags & VehicleFlag.NO_DEFLECTION_UP) ==0)
if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0)
force.Z += tmpV.Z;
}
}
// angular motor
if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000)
{
tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error
if (tmpV.LengthSquared() > 1e-6f)
{
tmpV = tmpV * (m_amEfect / m_angularMotorTimescale); // error to correct in this timestep
tmpV *= m_referenceFrame; // to object
dtorque.X += tmpV.X;
dtorque.Y += tmpV.Y;
dtorque.Z += tmpV.Z;
}
tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep
torque.X += tmpV.X;
torque.Y += tmpV.Y;
torque.Z += tmpV.Z;
m_amEfect *= (1 - 1.0f / m_angularMotorDecayTimescale);
}
else
@ -751,85 +751,64 @@ namespace OpenSim.Region.Physics.OdePlugin
// angular friction
if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0)
{
tmpV.X = -curLocalAngVel.X / m_angularFrictionTimescale.X;
tmpV.Y = -curLocalAngVel.Y / m_angularFrictionTimescale.Y;
tmpV.Z = -curLocalAngVel.Z / m_angularFrictionTimescale.Z;
tmpV *= m_referenceFrame; // to object
dtorque.X += tmpV.X;
dtorque.Y += tmpV.Y;
dtorque.Z += tmpV.Z;
torque.X -= curLocalAngVel.X / m_angularFrictionTimescale.X;
torque.Y -= curLocalAngVel.Y / m_angularFrictionTimescale.Y;
torque.Z -= curLocalAngVel.Z / m_angularFrictionTimescale.Z;
}
// angular deflection
if (m_angularDeflectionEfficiency > 0)
{
doathing = false;
float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale / m_angularDeflectionTimescale /_pParentScene.ODE_STEPSIZE;
tmpV.X = 0;
if (Math.Abs(curLocalVel.Z) > 0.01)
{
tmpV.Y = -(float)Math.Atan2(curLocalVel.Z, curLocalVel.X) * ftmp;
doathing = true;
}
else
tmpV.Y = 0;
if (Math.Abs(curLocalVel.Y) > 0.01)
{
tmpV.Z = (float)Math.Atan2(curLocalVel.Y, curLocalVel.X) * ftmp;
doathing = true;
}
else
tmpV.Z = 0;
Vector3 dirv;
if (doathing)
if (curLocalVel.X > 0.01f)
dirv = curLocalVel;
else if (curLocalVel.X < -0.01f)
// use oposite
dirv = -curLocalVel;
else
{
tmpV *= m_referenceFrame; // to object
dtorque.X += tmpV.X;
dtorque.Y += tmpV.Y;
dtorque.Z += tmpV.Z;
// make it fall into small positive x case
dirv.X = 0.01f;
dirv.Y = curLocalVel.Y;
dirv.Z = curLocalVel.Z;
}
float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale;
if (Math.Abs(dirv.Z) > 0.01)
{
torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp;
}
if (Math.Abs(dirv.Y) > 0.01)
{
torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp;
}
}
// vertical atractor
if (m_verticalAttractionTimescale < 300)
{
doathing = false;
float roll;
float pitch;
GetRollPitch(rotq, out roll, out pitch);
float ftmp = 1.0f / m_verticalAttractionTimescale / m_verticalAttractionTimescale / _pParentScene.ODE_STEPSIZE;
float ftmp2 = m_verticalAttractionEfficiency / _pParentScene.ODE_STEPSIZE;
if (Math.Abs(roll) > 0.01) // roll
{
tmpV.X = -roll * ftmp;
tmpV.X -= curLocalAngVel.X * ftmp2;
doathing = true;
}
else
{
tmpV.X = 0;
torque.X -= roll * ftmp + curLocalAngVel.X * ftmp2;
}
if (Math.Abs(pitch) > 0.01 && ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)) // pitch
{
tmpV.Y = -pitch * ftmp;
tmpV.Y -= curLocalAngVel.Y * ftmp2;
doathing = true;
}
else
{
tmpV.Y = 0;
torque.Y -= pitch * ftmp + curLocalAngVel.Y * ftmp2;
}
tmpV.Z = 0;
if (m_bankingEfficiency == 0 || Math.Abs(roll) < 0.01)
tmpV.Z = 0;
else
if (m_bankingEfficiency != 0 && Math.Abs(roll) < 0.01)
{
float broll = -roll * m_bankingEfficiency; ;
if (m_bankingMix != 0)
@ -839,145 +818,9 @@ namespace OpenSim.Region.Physics.OdePlugin
broll *= ((1 - m_bankingMix) + vfact);
}
tmpV.Z = (broll - curLocalAngVel.Z) / m_bankingTimescale;
doathing = true;
}
if (doathing)
{
tmpV *= m_referenceFrame; // to object
dtorque.X += tmpV.X;
dtorque.Y += tmpV.Y;
dtorque.Z += tmpV.Z;
torque.Z += (broll - curLocalAngVel.Z) / m_bankingTimescale;
}
}
/*
d.Vector3 pos = d.BodyGetPosition(Body);
// 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)
{
if (pos.X >= (m_BlockingEndPoint.X - (float)1))
{
pos.X -= posChange.X + 1;
d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
}
if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
{
pos.Y -= posChange.Y + 1;
d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
}
if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
{
pos.Z -= posChange.Z + 1;
d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
}
if (pos.X <= 0)
{
pos.X += posChange.X + 1;
d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
}
if (pos.Y <= 0)
{
pos.Y += posChange.Y + 1;
d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
}
}
if (pos.Z < _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y))
{
pos.Z = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + 2;
d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
}
}
if ((m_flags & (VehicleFlag.NO_X)) != 0)
{
m_dir.X = 0;
}
if ((m_flags & (VehicleFlag.NO_Y)) != 0)
{
m_dir.Y = 0;
}
if ((m_flags & (VehicleFlag.NO_Z)) != 0)
{
m_dir.Z = 0;
}
*/
// angular part
/*
// Get what the body is doing, this includes 'external' influences
/*
Vector3 angularVelocity = Vector3.Zero;
// Vertical attractor section
Vector3 vertattr = Vector3.Zero;
if (m_verticalAttractionTimescale < 300)
{
float VAservo = 0.2f / m_verticalAttractionTimescale;
// get present body rotation
// 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 = angularVelocity + vertattr; // + bank + deflection
if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
{
m_lastAngularVelocity.X = 0;
m_lastAngularVelocity.Y = 0;
}
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.
}
*/
d.Mass dmass;
d.BodyGetMass(Body,out dmass);
@ -988,8 +831,13 @@ namespace OpenSim.Region.Physics.OdePlugin
d.BodySetForce(Body, force.X, force.Y, force.Z);
}
if (dtorque.X != 0 || dtorque.Y != 0 || dtorque.Z != 0)
if (torque.X != 0 || torque.Y != 0 || torque.Z != 0)
{
torque *= m_referenceFrame; // to object frame
dtorque.X = torque.X;
dtorque.Y = torque.Y;
dtorque.Z = torque.Z;
d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque);
d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame
}

6
OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
View File

@ -1198,7 +1198,7 @@ namespace OpenSim.Region.Physics.OdePlugin
}
Body = IntPtr.Zero;
hasOOBoffsetFromMesh = false;
// CalcPrimBodyData();
CalcPrimBodyData();
}
private void ChildSetGeom(OdePrim odePrim)
@ -1223,7 +1223,6 @@ namespace OpenSim.Region.Physics.OdePlugin
{
if (m_isphysical && Body != IntPtr.Zero && prim_geom != IntPtr.Zero)
{
/*
if (m_targetSpace != _parent_scene.ActiveSpace)
{
m_targetSpace = _parent_scene.ActiveSpace;
@ -1238,7 +1237,6 @@ namespace OpenSim.Region.Physics.OdePlugin
}
d.SpaceAdd(m_targetSpace, prim_geom);
}
*/
d.GeomEnable(prim_geom);
foreach (OdePrim prm in childrenPrim)
d.GeomEnable(prm.prim_geom);
@ -1256,7 +1254,6 @@ namespace OpenSim.Region.Physics.OdePlugin
{
if (m_isphysical && Body != IntPtr.Zero && prim_geom != IntPtr.Zero)
{
/*
if (m_targetSpace == _parent_scene.ActiveSpace)
{
foreach (OdePrim prm in childrenPrim)
@ -1270,7 +1267,6 @@ namespace OpenSim.Region.Physics.OdePlugin
d.SpaceRemove(m_targetSpace, prim_geom);
m_targetSpace = IntPtr.Zero;
}
*/
d.GeomDisable(prim_geom);
foreach (OdePrim prm in childrenPrim)
d.GeomDisable(prm.prim_geom);

81
OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs
View File

@ -59,6 +59,7 @@ namespace OdeAPI
{
public static dReal Infinity = dReal.MaxValue;
public static int NTotalBodies = 0;
public static int NTotalGeoms = 0;
#region Flags and Enumerations
@ -689,22 +690,52 @@ namespace OdeAPI
public static extern IntPtr ConnectingJoint(IntPtr j1, IntPtr j2);
[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateBox"), SuppressUnmanagedCodeSecurity]
public static extern IntPtr CreateBox(IntPtr space, dReal lx, dReal ly, dReal lz);
public static extern IntPtr CreateiBox(IntPtr space, dReal lx, dReal ly, dReal lz);
public static IntPtr CreateBox(IntPtr space, dReal lx, dReal ly, dReal lz)
{
NTotalGeoms++;
return CreateiBox(space, lx, ly, lz);
}
[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCapsule"), SuppressUnmanagedCodeSecurity]
public static extern IntPtr CreateCapsule(IntPtr space, dReal radius, dReal length);
public static extern IntPtr CreateiCapsule(IntPtr space, dReal radius, dReal length);
public static IntPtr CreateCapsule(IntPtr space, dReal radius, dReal length)
{
NTotalGeoms++;
return CreateiCapsule(space, radius, length);
}
[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateConvex"), SuppressUnmanagedCodeSecurity]
public static extern IntPtr CreateConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
public static extern IntPtr CreateiConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
public static IntPtr CreateConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons)
{
NTotalGeoms++;
return CreateiConvex(space, planes, planeCount, points, pointCount, polygons);
}
[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCylinder"), SuppressUnmanagedCodeSecurity]
public static extern IntPtr CreateCylinder(IntPtr space, dReal radius, dReal length);
public static extern IntPtr CreateiCylinder(IntPtr space, dReal radius, dReal length);
public static IntPtr CreateCylinder(IntPtr space, dReal radius, dReal length)
{
NTotalGeoms++;
return CreateiCylinder(space, radius, length);
}
[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateHeightfield"), SuppressUnmanagedCodeSecurity]
public static extern IntPtr CreateHeightfield(IntPtr space, IntPtr data, int bPlaceable);
public static extern IntPtr CreateiHeightfield(IntPtr space, IntPtr data, int bPlaceable);
public static IntPtr CreateHeightfield(IntPtr space, IntPtr data, int bPlaceable)
{
NTotalGeoms++;
return CreateiHeightfield(space, data, bPlaceable);
}
[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeom"), SuppressUnmanagedCodeSecurity]
public static extern IntPtr CreateGeom(int classnum);
public static extern IntPtr CreateiGeom(int classnum);
public static IntPtr CreateGeom(int classnum)
{
NTotalGeoms++;
return CreateiGeom(classnum);
}
[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomClass"), SuppressUnmanagedCodeSecurity]
public static extern int CreateGeomClass(ref GeomClass classptr);
@ -713,18 +744,38 @@ namespace OdeAPI
public static extern IntPtr CreateGeomTransform(IntPtr space);
[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreatePlane"), SuppressUnmanagedCodeSecurity]
public static extern IntPtr CreatePlane(IntPtr space, dReal a, dReal b, dReal c, dReal d);
public static extern IntPtr CreateiPlane(IntPtr space, dReal a, dReal b, dReal c, dReal d);
public static IntPtr CreatePlane(IntPtr space, dReal a, dReal b, dReal c, dReal d)
{
NTotalGeoms++;
return CreateiPlane(space, a, b, c, d);
}
[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateRay"), SuppressUnmanagedCodeSecurity]
public static extern IntPtr CreateRay(IntPtr space, dReal length);
public static extern IntPtr CreateiRay(IntPtr space, dReal length);
public static IntPtr CreateRay(IntPtr space, dReal length)
{
NTotalGeoms++;
return CreateiRay(space, length);
}
[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateSphere"), SuppressUnmanagedCodeSecurity]
public static extern IntPtr CreateSphere(IntPtr space, dReal radius);
public static extern IntPtr CreateiSphere(IntPtr space, dReal radius);
public static IntPtr CreateSphere(IntPtr space, dReal radius)
{
NTotalGeoms++;
return CreateiSphere(space, radius);
}
[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateTriMesh"), SuppressUnmanagedCodeSecurity]
public static extern IntPtr CreateTriMesh(IntPtr space, IntPtr data,
public static extern IntPtr CreateiTriMesh(IntPtr space, IntPtr data,
TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback);
public static IntPtr CreateTriMesh(IntPtr space, IntPtr data,
TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback)
{
NTotalGeoms++;
return CreateiTriMesh(space, data, callback, arrayCallback, rayCallback);
}
[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDot"), SuppressUnmanagedCodeSecurity]
public static extern dReal Dot(ref dReal X0, ref dReal X1, int n);
@ -798,7 +849,13 @@ namespace OdeAPI
public static extern void GeomCylinderSetParams(IntPtr geom, dReal radius, dReal length);
[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDestroy"), SuppressUnmanagedCodeSecurity]
public static extern void GeomDestroy(IntPtr geom);
public static extern void GeomiDestroy(IntPtr geom);
public static void GeomDestroy(IntPtr geom)
{
NTotalGeoms--;
GeomiDestroy(geom);
}
[DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDisable"), SuppressUnmanagedCodeSecurity]
public static extern void GeomDisable(IntPtr geom);

1
OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
View File

@ -1860,6 +1860,7 @@ namespace OpenSim.Region.Physics.OdePlugin
int nstaticgeoms = d.SpaceGetNumGeoms(StaticSpace);
int ntopgeoms = d.SpaceGetNumGeoms(TopSpace);
int nbodies = d.NTotalBodies;
int ngeoms = d.NTotalGeoms;
// Finished with all sim stepping. If requested, dump world state to file for debugging.
// TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?