public PhysicsScene GetScene(String sceneIdentifier)
{
if (m_scene == null)
{
m_scene = new OdeScene(sceneIdentifier);
}
return (m_scene);
}
public PhysicsScene GetScene(String sceneIdentifier)
{
if (_mScene == null)
{
// Initializing ODE only when a scene is created allows alternative ODE plugins to co-habit (according to
// http://opensimulator.org/mantis/view.php?id=2750).
d.InitODE();
_mScene = new OdeScene(ode, sceneIdentifier);
}
return (_mScene);
}
public PhysicsScene GetScene(String sceneIdentifier)
{
if (m_scene == null)
{
// We do this so that OpenSimulator on Windows loads the correct native ODE library depending on whether
// it's running as a 32-bit process or a 64-bit one. By invoking LoadLibary here, later DLLImports
// will find it already loaded later on.
//
// This isn't necessary for other platforms (e.g. Mac OSX and Linux) since the DLL used can be
// controlled in Ode.NET.dll.config
if (Util.IsWindows())
Util.LoadArchSpecificWindowsDll("ode.dll");
// Initializing ODE only when a scene is created allows alternative ODE plugins to co-habit (according to
// http://opensimulator.org/mantis/view.php?id=2750).
d.InitODE();
m_scene = new OdeScene(GetName(), sceneIdentifier);
}
return m_scene;
}
public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, CollisionLocker dode, Vector3 size, float pid_d, float pid_p, float capsule_radius, float tensor, float density, float height_fudge_factor, float walk_divisor, float rundivisor)
{
m_uuid = UUID.Random();
if (pos.IsFinite())
{
if (pos.Z > 9999999f)
{
pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
}
if (pos.Z < -90000f)
{
pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
}
_position = pos;
m_taintPosition.X = pos.X;
m_taintPosition.Y = pos.Y;
m_taintPosition.Z = pos.Z;
}
else
{
_position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f);
m_taintPosition.X = _position.X;
m_taintPosition.Y = _position.Y;
m_taintPosition.Z = _position.Z;
m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
}
_parent_scene = parent_scene;
PID_D = pid_d;
PID_P = pid_p;
CAPSULE_RADIUS = capsule_radius;
m_tensor = tensor;
m_density = density;
heightFudgeFactor = height_fudge_factor;
walkDivisor = walk_divisor;
runDivisor = rundivisor;
// m_StandUpRotation =
// new d.Matrix3(0.5f, 0.7071068f, 0.5f, -0.7071068f, 0f, 0.7071068f, 0.5f, -0.7071068f,
// 0.5f);
for (int i = 0; i < 11; i++)
{
m_colliderarr[i] = false;
}
CAPSULE_LENGTH = (size.Z * 1.15f) - CAPSULE_RADIUS * 2.0f;
//m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
m_tainted_CAPSULE_LENGTH = CAPSULE_LENGTH;
m_isPhysical = false; // current status: no ODE information exists
m_tainted_isPhysical = true; // new tainted status: need to create ODE information
_parent_scene.AddPhysicsActorTaint(this);
m_name = avName;
}
public ODERayCastRequestManager(OdeScene pScene)
{
m_scene = pScene;
nearCallback = near;
ray = d.CreateRay(IntPtr.Zero, 1.0f);
}
public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical)
{
Name = primName;
m_vehicle = null;
if (!pos.IsFinite())
{
pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name);
}
_position = pos;
givefakepos = 0;
PID_D = parent_scene.bodyPIDD;
PID_G = parent_scene.bodyPIDG;
m_density = parent_scene.geomDefaultDensity;
// m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
prim_geom = IntPtr.Zero;
Body = IntPtr.Zero;
if (!size.IsFinite())
{
size = new Vector3(0.5f, 0.5f, 0.5f);
m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name);
}
if (size.X <= 0) size.X = 0.01f;
if (size.Y <= 0) size.Y = 0.01f;
if (size.Z <= 0) size.Z = 0.01f;
_size = size;
if (!QuaternionIsFinite(rotation))
{
rotation = Quaternion.Identity;
m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name);
}
_orientation = rotation;
givefakeori = 0;
_pbs = pbs;
_parent_scene = parent_scene;
m_targetSpace = IntPtr.Zero;
if (pos.Z < 0)
{
m_isphysical = false;
}
else
{
m_isphysical = pisPhysical;
}
m_fakeisphysical = m_isphysical;
m_isVolumeDetect = false;
m_force = Vector3.Zero;
m_iscolliding = false;
m_wascolliding = false;
m_colliderfilter = 0;
hasOOBoffsetFromMesh = false;
_triMeshData = IntPtr.Zero;
primContactData.mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu;
primContactData.bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce;
CalcPrimBodyData();
m_building = true; // control must set this to false when done
AddChange(changes.Add, null);
}
internal void Enable(IntPtr pBody, OdeScene pParentScene)
{
if (m_type == Vehicle.TYPE_NONE)
return;
m_body = pBody;
// m_parentScene = pParentScene;
if (m_jointGroup == IntPtr.Zero)
m_jointGroup = d.JointGroupCreate(3);
if (pBody != IntPtr.Zero)
{
if (m_lMotor1 == IntPtr.Zero)
{
d.BodySetAutoDisableFlag(Body, false);
m_lMotor1 = d.JointCreateLMotor(pParentScene.world, m_jointGroup);
d.JointSetLMotorNumAxes(m_lMotor1, 1);
d.JointAttach(m_lMotor1, Body, IntPtr.Zero);
}
if (m_aMotor == IntPtr.Zero)
{
m_aMotor = d.JointCreateAMotor(pParentScene.world, m_jointGroup);
d.JointSetAMotorNumAxes(m_aMotor, 3);
d.JointAttach(m_aMotor, Body, IntPtr.Zero);
}
}
}
public void setMesh(OdeScene parent_scene, IMesh mesh)
{
// m_log.DebugFormat("[ODE PRIM]: Setting mesh on {0} to {1}", Name, mesh);
// This sleeper is there to moderate how long it takes between
// setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object
//Thread.Sleep(10);
//Kill Body so that mesh can re-make the geom
if (IsPhysical && Body != IntPtr.Zero)
{
if (childPrim)
{
if (_parent != null)
{
OdePrim parent = (OdePrim)_parent;
parent.ChildDelink(this);
}
}
else
{
disableBody();
}
}
IntPtr vertices, indices;
int vertexCount, indexCount;
int vertexStride, triStride;
mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
if (m_MeshToTriMeshMap.ContainsKey(mesh))
{
_triMeshData = m_MeshToTriMeshMap[mesh];
}
else
{
_triMeshData = d.GeomTriMeshDataCreate();
d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
d.GeomTriMeshDataPreprocess(_triMeshData);
m_MeshToTriMeshMap[mesh] = _triMeshData;
}
_parent_scene.waitForSpaceUnlock(m_targetSpace);
try
{
if (prim_geom == IntPtr.Zero)
{
SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
}
}
catch (AccessViolationException)
{
m_log.ErrorFormat("[PHYSICS]: MESH LOCKED FOR {0}", Name);
return;
}
// if (IsPhysical && Body == (IntPtr) 0)
// {
// Recreate the body
// m_interpenetrationcount = 0;
// m_collisionscore = 0;
// enableBody();
// }
}
public ODERayCastRequestManager(OdeScene pScene)
{
m_scene = pScene;
nearCallback = near;
}
internal void Step(float pTimestep, OdeScene pParentScene)
{
if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
return;
frcount++; // used to limit debug comment output
if (frcount > 100)
frcount = 0;
MoveLinear(pTimestep, pParentScene);
MoveAngular(pTimestep);
LimitRotation(pTimestep);
}// end Step
}//end SetDefaultsForType
internal void Enable(IntPtr pBody, OdeScene pParentScene)
{
if (m_type == Vehicle.TYPE_NONE)
return;
m_body = pBody;
}
public void setMesh(OdeScene parent_scene, IMesh mesh)
{
// This sleeper is there to moderate how long it takes between
// setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object
Thread.Sleep(10);
//Kill Body so that mesh can re-make the geom
if (IsPhysical && Body != IntPtr.Zero)
{
if (childPrim)
{
if (_parent != null)
{
OdePrim parent = (OdePrim)_parent;
parent.ChildDelink(this);
}
}
else
{
disableBody();
}
}
IMesh oldMesh = primMesh;
primMesh = mesh;
float[] vertexList = primMesh.getVertexListAsFloatLocked(); // Note, that vertextList is pinned in memory
int[] indexList = primMesh.getIndexListAsIntLocked(); // Also pinned, needs release after usage
primMesh.releaseSourceMeshData(); // free up the original mesh data to save memory
int VertexCount = vertexList.GetLength(0)/3;
int IndexCount = indexList.GetLength(0);
_triMeshData = d.GeomTriMeshDataCreate();
d.GeomTriMeshDataBuildSimple(_triMeshData, vertexList, 3*sizeof (float), VertexCount, indexList, IndexCount,
3*sizeof (int));
d.GeomTriMeshDataPreprocess(_triMeshData);
_parent_scene.waitForSpaceUnlock(m_targetSpace);
try
{
if (prim_geom == IntPtr.Zero)
{
SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
}
}
catch (AccessViolationException)
{
m_log.Error("[PHYSICS]: MESH LOCKED");
return;
}
if (oldMesh != null)
{
oldMesh.releasePinned();
oldMesh = null;
}
// if (IsPhysical && Body == (IntPtr) 0)
// {
// Recreate the body
// m_interpenetrationcount = 0;
// m_collisionscore = 0;
// enableBody();
// }
}
public OdePrim(String primName, OdeScene parent_scene, PhysicsVector pos, PhysicsVector size,
Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode)
{
_target_velocity = new PhysicsVector(0, 0, 0);
m_vehicle = new ODEVehicleSettings();
//gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
ode = dode;
_velocity = new PhysicsVector();
if (!PhysicsVector.isFinite(pos))
{
pos = new PhysicsVector(128, 128, parent_scene.GetTerrainHeightAtXY(128, 128) + 0.5f);
m_log.Warn("[PHYSICS]: Got nonFinite Object create Position");
}
_position = pos;
m_taintposition = pos;
PID_D = parent_scene.bodyPIDD;
PID_G = parent_scene.bodyPIDG;
m_density = parent_scene.geomDefaultDensity;
// m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
prim_geom = IntPtr.Zero;
prev_geom = IntPtr.Zero;
if (!PhysicsVector.isFinite(pos))
{
size = new PhysicsVector(0.5f, 0.5f, 0.5f);
m_log.Warn("[PHYSICS]: Got nonFinite Object create Size");
}
if (size.X <= 0) size.X = 0.01f;
if (size.Y <= 0) size.Y = 0.01f;
if (size.Z <= 0) size.Z = 0.01f;
_size = size;
m_taintsize = _size;
_acceleration = new PhysicsVector();
m_rotationalVelocity = PhysicsVector.Zero;
if (!QuaternionIsFinite(rotation))
{
rotation = Quaternion.Identity;
m_log.Warn("[PHYSICS]: Got nonFinite Object create Rotation");
}
_orientation = rotation;
m_taintrot = _orientation;
_mesh = mesh;
_pbs = pbs;
_parent_scene = parent_scene;
m_targetSpace = (IntPtr)0;
if (pos.Z < 0)
m_isphysical = false;
else
{
m_isphysical = pisPhysical;
// If we're physical, we need to be in the master space for now.
// linksets *should* be in a space together.. but are not currently
if (m_isphysical)
m_targetSpace = _parent_scene.space;
}
m_primName = primName;
m_taintadd = true;
_parent_scene.AddPhysicsActorTaint(this);
// don't do .add() here; old geoms get recycled with the same hash
}
}// end Step
private void MoveLinear(float pTimestep, OdeScene _pParentScene)
{
if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant
{
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?
// 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))
m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X;
if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y))
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);
m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f;
//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
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
// 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);
// apply gravity force
d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
// apply friction
Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
} // end MoveLinear()
}//end SetDefaultsForType
internal void Step(float pTimestep, OdeScene pParentScene,IntPtr m_body)
{
frcount++; // used to limit debug comment output
if (frcount > 100)
frcount = 0;
// scale time so thing are similar as before and scripts don't break
pTimestep *= 0.09375f / pParentScene.ODE_STEPSIZE;
MoveLinear(pTimestep, pParentScene, m_body);
MoveAngular(pTimestep, m_body);
LimitRotation(pTimestep, m_body);
}// end Step
/// <summary>
/// Dereference the creator scene so that it can be garbage collected if needed.
/// </summary>
internal void Dispose()
{
m_scene = null;
}
}// end Step
private void MoveLinear(float pTimestep, OdeScene _pParentScene)
{
if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant
{
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?
// 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))
m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X;
if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y))
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);
m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f;
//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
m_dir = m_lastLinearVelocityVector;
d.Quaternion rot = d.BodyGetQuaternion(Body);
Quaternion rotq = new Quaternion((float)rot.X, (float)rot.Y, (float)rot.Z, (float)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.
Vector3 grav = Vector3.Zero;
// 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 = (float)(_pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy));
// Preserve the current Z velocity
d.Vector3 vel_now = d.BodyGetLinearVel(Body);
m_dir.Z = (float)vel_now.Z; // Preserve the accumulated falling velocity
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 = (float)(pos.X - m_lastPositionVector.X);
posChange.Y = (float)(pos.Y - m_lastPositionVector.Y);
posChange.Z = (float)(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((float)pos.X, (float)pos.Y))
{
pos.Z = _pParentScene.GetTerrainHeightAtXY((float)pos.X, (float)pos.Y) + 2;
d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
}
// Check if hovering
if ((m_Hoverflags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
{
// We should hover, get the target height
if ((m_Hoverflags & VehicleFlag.HOVER_WATER_ONLY) != 0)
{
m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight;
}
if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
{
m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY((float)pos.X, (float)pos.Y) + m_VhoverHeight;
}
if ((m_Hoverflags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
{
m_VhoverTargetHeight = m_VhoverHeight;
}
if ((m_Hoverflags & VehicleFlag.HOVER_UP_ONLY) != 0)
{
// If body is aready heigher, use its height as target height
if (pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = (float)pos.Z;
}
if ((m_Hoverflags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
{
if ((pos.Z - m_VhoverTargetHeight) > .2 || (pos.Z - m_VhoverTargetHeight) < -.2)
{
d.BodySetPosition(Body, pos.X, pos.Y, m_VhoverTargetHeight);
}
}
else
{
float herr0 = (float)(pos.Z - m_VhoverTargetHeight);
// Replace Vertical speed with correction figure if significant
if (Math.Abs(herr0) > 0.01f)
{
m_dir.Z = -((herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
//KF: m_VhoverEfficiency is not yet implemented
}
else
{
m_dir.Z = 0f;
}
}
// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
// m_VhoverTimescale = 0f; // time to acheive height
// pTimestep is time since last frame,in secs
}
if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
{
//Start Experimental Values
if (Zchange > .3)
{
grav.Z = (float)(grav.Z * 3);
}
if (Zchange > .15)
{
grav.Z = (float)(grav.Z * 2);
}
if (Zchange > .75)
{
grav.Z = (float)(grav.Z * 1.5);
}
if (Zchange > .05)
{
grav.Z = (float)(grav.Z * 1.25);
}
if (Zchange > .025)
{
grav.Z = (float)(grav.Z * 1.125);
}
float terraintemp = _pParentScene.GetTerrainHeightAtXY((float)pos.X, (float)pos.Y);
float postemp = (float)(pos.Z - terraintemp);
if (postemp > 2.5f)
{
grav.Z = (float)(grav.Z * 1.037125);
}
//End Experimental Values
}
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;
}
m_lastPositionVector = d.BodyGetPosition(Body);
// 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);
// apply friction
Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
} // end MoveLinear()
public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode)
{
Name = primName;
m_vehicle = new ODEDynamics();
//gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
ode = dode;
if (!pos.IsFinite())
{
pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name);
}
_position = pos;
m_taintposition = pos;
PID_D = parent_scene.bodyPIDD;
PID_G = parent_scene.bodyPIDG;
m_density = parent_scene.geomDefaultDensity;
// m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
prim_geom = IntPtr.Zero;
if (!pos.IsFinite())
{
size = new Vector3(0.5f, 0.5f, 0.5f);
m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name);
}
if (size.X <= 0) size.X = 0.01f;
if (size.Y <= 0) size.Y = 0.01f;
if (size.Z <= 0) size.Z = 0.01f;
_size = size;
m_taintsize = _size;
if (!QuaternionIsFinite(rotation))
{
rotation = Quaternion.Identity;
m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name);
}
_orientation = rotation;
m_taintrot = _orientation;
_pbs = pbs;
_parent_scene = parent_scene;
m_targetSpace = (IntPtr)0;
if (pos.Z < 0)
{
IsPhysical = false;
}
else
{
IsPhysical = pisPhysical;
// If we're physical, we need to be in the master space for now.
// linksets *should* be in a space together.. but are not currently
if (IsPhysical)
m_targetSpace = _parent_scene.space;
}
m_taintadd = true;
_parent_scene.AddPhysicsActorTaint(this);
// don't do .add() here; old geoms get recycled with the same hash
}
public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, Vector3 size, float pid_d, float pid_p, float capsule_radius, float density, float walk_divisor, float rundivisor)
{
m_uuid = UUID.Random();
m_hasTaintPosition = false;
if (pos.IsFinite())
{
if (pos.Z > 9999999f)
{
pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
}
if (pos.Z < -90000f)
{
pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
}
_position = pos;
}
else
{
_position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f);
m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
}
_parent_scene = parent_scene;
PID_D = pid_d;
PID_P = pid_p;
CAPSULE_RADIUS = capsule_radius;
m_density = density;
m_mass = 80f; // sure we have a default
AvatarContactData.mu = parent_scene.AvatarFriction;
AvatarContactData.bounce = parent_scene.AvatarBounce;
walkDivisor = walk_divisor;
runDivisor = rundivisor;
CAPSULE_LENGTH = size.Z * 1.15f - CAPSULE_RADIUS * 2.0f;
//m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
m_tainted_CAPSULE_LENGTH = CAPSULE_LENGTH;
m_isPhysical = false; // current status: no ODE information exists
m_tainted_isPhysical = true; // new tainted status: need to create ODE information
m_hasTaintForce = false;
_parent_scene.AddPhysicsActorTaint(this);
m_name = avName;
}
/// <summary>
/// ODE Avatar.
/// </summary>
/// <param name="avName"></param>
/// <param name="parent_scene"></param>
/// <param name="pos"></param>
/// <param name="size"></param>
/// <param name="pid_d"></param>
/// <param name="pid_p"></param>
/// <param name="capsule_radius"></param>
/// <param name="tensor"></param>
/// <param name="density">
/// Only used right now to return information to LSL. Not actually used to set mass in ODE!
/// </param>
/// <param name="walk_divisor"></param>
/// <param name="rundivisor"></param>
public OdeCharacter(
String avName, OdeScene parent_scene, Vector3 pos, Vector3 size, float pid_d, float pid_p,
float capsule_radius, float tensor, float density,
float walk_divisor, float rundivisor)
{
m_uuid = UUID.Random();
if (pos.IsFinite())
{
if (pos.Z > 9999999f)
{
pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
}
if (pos.Z < -90000f)
{
pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
}
_position = pos;
m_taintPosition = pos;
}
else
{
_position
= new Vector3(
(float)_parent_scene.WorldExtents.X * 0.5f,
(float)_parent_scene.WorldExtents.Y * 0.5f,
parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f);
m_taintPosition = _position;
m_log.WarnFormat("[ODE CHARACTER]: Got NaN Position on Character Create for {0}", avName);
}
_parent_scene = parent_scene;
PID_D = pid_d;
PID_P = pid_p;
CAPSULE_RADIUS = capsule_radius;
m_tensor = tensor;
m_density = density;
// heightFudgeFactor = height_fudge_factor;
walkDivisor = walk_divisor;
runDivisor = rundivisor;
// m_StandUpRotation =
// new d.Matrix3(0.5f, 0.7071068f, 0.5f, -0.7071068f, 0f, 0.7071068f, 0.5f, -0.7071068f,
// 0.5f);
// We can set taint and actual to be the same here, since the entire character will be set up when the
// m_tainted_isPhysical is processed.
SetTaintedCapsuleLength(size);
CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH;
m_isPhysical = false; // current status: no ODE information exists
m_tainted_isPhysical = true; // new tainted status: need to create ODE information
_parent_scene.AddPhysicsActorTaint(this);
Name = avName;
}
private bool setMesh(OdeScene parent_scene, IMesh mesh)
{
if (Body != IntPtr.Zero)
{
if (childPrim)
{
if (_parent != null)
{
OdePrim parent = (OdePrim)_parent;
parent.ChildDelink(this);
}
}
else
{
DestroyBody();
}
}
IntPtr vertices, indices;
int vertexCount, indexCount;
int vertexStride, triStride;
mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
if (vertexCount == 0 || indexCount == 0)
{
m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. It can be a sculp with alpha channel in map. Replacing it by a small box.", Name, _position.X, _position.Y, _position.Z);
_size.X = 0.01f;
_size.Y = 0.01f;
_size.Z = 0.01f;
return false;
}
primOOBoffset = mesh.GetCentroid();
hasOOBoffsetFromMesh = true;
_triMeshData = d.GeomTriMeshDataCreate();
d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
d.GeomTriMeshDataPreprocess(_triMeshData);
mesh.releaseSourceMeshData();
_parent_scene.waitForSpaceUnlock(m_targetSpace);
try
{
SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null));
}
catch (Exception e)
{
m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e);
return false;
}
return true;
}
