public void getWorldTransform(out btTransform result)
{
if (m_disposed)
throw new ObjectDisposedException(ToString());
result = btTransform.getIdentity();
BulletAPI_BtDefaultMotionState_getWorldTransform(m_handle, result.Handle);
}
public void setWorldTransform(btTransform t)
{
if (m_disposed)
{
throw new ObjectDisposedException(ToString());
}
BulletAPI_BtDefaultMotionState_setWorldTransform(m_handle, t.Handle);
}
public btTransform inverseTimes(btTransform t)
{
if (m_disposed)
{
throw new ObjectDisposedException(ToString());
}
return(FromIntPtr(BulletAPI_BtTransform_inverseTimes(m_handle, t.Handle)));
}
public void setWorldTransform(btTransform worldTrans)
{
if (m_disposed)
{
throw new ObjectDisposedException(ToString());
}
BulletAPI_BtCollisionObject_setWorldTransform(m_handle, worldTrans.Handle);
}
public void mult(btTransform t1, btTransform t2)
{
if (m_disposed)
{
throw new ObjectDisposedException(ToString());
}
BulletAPI_BtTransform_mult(m_handle, t1.Handle, t2.Handle);
}
public void getWorldTransform(out btTransform result)
{
if (m_disposed)
{
throw new ObjectDisposedException(ToString());
}
result = btTransform.getIdentity();
BulletAPI_BtDefaultMotionState_getWorldTransform(m_handle, result.Handle);
}
public override void SetTerrain(bool[] heightMap)
{
if (m_terrainShape != null)
DeleteTerrain();
m_terrainShape = AddPrim(
"__TERRAIN__",
new Vector3(Constants.RegionSize / 2, Constants.RegionSize / 2, Constants.RegionSize / 2),
new Vector3(Constants.RegionSize, Constants.RegionSize, Constants.RegionSize),
Quaternion.Identity,
voxmesher.ToMesh(heightMap),
PrimitiveBaseShape.Default,
false);
//float AabbCenterX = Constants.RegionSize/2f;
//float AabbCenterY = Constants.RegionSize/2f;
//float AabbCenterZ = 0f;
/*
float temphfmin, temphfmax;
temphfmin = hfmin;
temphfmax = hfmax;
if (temphfmin < 0)
{
temphfmax = 0 - temphfmin;
temphfmin = 0 - temphfmin;
}
else if (temphfmin > 0)
{
temphfmax = temphfmax + (0 - temphfmin);
//temphfmin = temphfmin + (0 - temphfmin);
}
AabbCenterZ = temphfmax/2f;
if (m_terrainPosition == null)
{
m_terrainPosition = new btVector3(AabbCenterX, AabbCenterY, AabbCenterZ);
}
else
{
try
{
m_terrainPosition.setValue(AabbCenterX, AabbCenterY, AabbCenterZ);
}
catch (ObjectDisposedException)
{
m_terrainPosition = new btVector3(AabbCenterX, AabbCenterY, AabbCenterZ);
}
}
*/
if (m_terrainMotionState != null)
{
m_terrainMotionState.Dispose();
m_terrainMotionState = null;
}
m_terrainTransform = new btTransform(QuatIdentity, m_terrainPosition);
m_terrainMotionState = new btDefaultMotionState(m_terrainTransform);
}
/// <summary>
/// Called from Simulate
/// This is the avatar's movement control + PID Controller
/// </summary>
/// <param name="timeStep"></param>
public void Move(float timeStep)
{
// 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 (Body == null)
return;
tempTrans1.Dispose();
tempTrans1 = Body.getInterpolationWorldTransform();
tempVector1.Dispose();
tempVector1 = tempTrans1.getOrigin();
tempVector2.Dispose();
tempVector2 = Body.getInterpolationLinearVelocity();
if (m_pidControllerActive == false)
{
m_zeroPosition.X = tempVector1.getX();
m_zeroPosition.Y = tempVector1.getY();
m_zeroPosition.Z = tempVector1.getZ();
}
//PidStatus = true;
Vector3 vec = Vector3.Zero;
Vector3 vel = new Vector3(tempVector2.getX(), tempVector2.getY(), tempVector2.getZ());
float movementdivisor = 1f;
if (!m_alwaysRun)
{
movementdivisor = walkDivisor;
}
else
{
movementdivisor = runDivisor;
}
// if velocity is zero, use position control; otherwise, velocity control
if (m_target_velocity.X == 0.0f && m_target_velocity.Y == 0.0f && m_target_velocity.Z == 0.0f && m_iscolliding)
{
// keep track of where we stopped. No more slippin' & slidin'
if (!m_zeroFlag)
{
m_zeroFlag = true;
m_zeroPosition.X = tempVector1.getX();
m_zeroPosition.Y = tempVector1.getY();
m_zeroPosition.Z = tempVector1.getZ();
}
if (m_pidControllerActive)
{
// 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
Vector3 pos = new Vector3(tempVector1.getX(), tempVector1.getY(), tempVector1.getZ());
vec.X = (m_target_velocity.X - vel.X) * (PID_D) + (m_zeroPosition.X - pos.X) * (PID_P * 2);
vec.Y = (m_target_velocity.Y - vel.Y) * (PID_D) + (m_zeroPosition.Y - pos.Y) * (PID_P * 2);
if (m_flying)
{
vec.Z = (m_target_velocity.Z - vel.Z) * (PID_D) + (m_zeroPosition.Z - pos.Z) * PID_P;
}
}
//PidStatus = true;
}
else
{
m_pidControllerActive = true;
m_zeroFlag = false;
if (m_iscolliding && !m_flying)
{
// We're standing on something
vec.X = ((m_target_velocity.X / movementdivisor) - vel.X) * (PID_D);
vec.Y = ((m_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D);
}
else if (m_iscolliding && m_flying)
{
// We're flying and colliding with something
vec.X = ((m_target_velocity.X / movementdivisor) - vel.X) * (PID_D / 16);
vec.Y = ((m_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D / 16);
}
else if (!m_iscolliding && m_flying)
{
// we're in mid air suspended
vec.X = ((m_target_velocity.X / movementdivisor) - vel.X) * (PID_D / 6);
vec.Y = ((m_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D / 6);
// We don't want linear velocity to cause our avatar to bounce, so we check target Z and actual velocity X, Y
// rebound preventing
if (m_target_velocity.Z < 0.025f && m_velocity.X < 0.25f && m_velocity.Y < 0.25f)
m_zeroFlag = true;
}
if (m_iscolliding && !m_flying && m_target_velocity.Z > 0.0f)
{
// We're colliding with something and we're not flying but we're moving
// This means we're walking or running.
Vector3 pos = new Vector3(tempVector1.getX(), tempVector1.getY(), tempVector1.getZ());
vec.Z = (m_target_velocity.Z - vel.Z) * PID_D + (m_zeroPosition.Z - pos.Z) * PID_P;
if (m_target_velocity.X > 0)
{
vec.X = ((m_target_velocity.X - vel.X) / 1.2f) * PID_D;
}
if (m_target_velocity.Y > 0)
{
vec.Y = ((m_target_velocity.Y - vel.Y) / 1.2f) * PID_D;
}
}
else if (!m_iscolliding && !m_flying)
{
// we're not colliding and we're not flying so that means we're falling!
// m_iscolliding includes collisions with the ground.
// d.Vector3 pos = d.BodyGetPosition(Body);
if (m_target_velocity.X > 0)
{
vec.X = ((m_target_velocity.X - vel.X) / 1.2f) * PID_D;
}
if (m_target_velocity.Y > 0)
{
vec.Y = ((m_target_velocity.Y - vel.Y) / 1.2f) * PID_D;
}
}
if (m_flying)
{
vec.Z = (m_target_velocity.Z - vel.Z) * (PID_D);
}
}
if (m_flying)
{
// Slight PID correction
vec.Z += (((-1 * m_parent_scene.gravityz) * m_mass) * 0.06f);
//auto fly height. Kitto Flora
//d.Vector3 pos = d.BodyGetPosition(Body);
float target_altitude = m_parent_scene.GetTerrainHeightAtXY(m_position.X, m_position.Y) + m_parent_scene.minimumGroundFlightOffset;
if (m_position.Z < target_altitude)
{
vec.Z += (target_altitude - m_position.Z) * PID_P * 5.0f;
}
}
if (Body != null && (((m_target_velocity.X > 0.2f || m_target_velocity.X < -0.2f) || (m_target_velocity.Y > 0.2f || m_target_velocity.Y < -0.2f))))
{
Body.setFriction(0.001f);
//m_log.DebugFormat("[PHYSICS]: Avatar force applied: {0}, Target:{1}", vec.ToString(), m_target_velocity.ToString());
}
if (Body != null)
{
int activationstate = Body.getActivationState();
if (activationstate == 0)
{
Body.forceActivationState(1);
}
}
doImpulse(vec, true);
}
public void setWorldTransform(btTransform t)
{
if (m_disposed)
throw new ObjectDisposedException(ToString());
BulletAPI_BtDefaultMotionState_setWorldTransform(m_handle, t.Handle);
}
public btGeneric6DofConstraint(btRigidBody rbA, btRigidBody rbB, btTransform frameInA, btTransform frameInB, bool useLinearReferenceFrameA)
: base()
{
m_handle = BulletAPI_CreateBtGeneric6DofConstraint(rbA.Handle, rbB.Handle, frameInA.Handle, frameInB.Handle, useLinearReferenceFrameA);
}
public override void SetTerrain(float[] heightMap, double[,] normalHeightMap)
{
if (m_terrainShape != null)
DeleteTerrain();
float hfmax = -9000;
float hfmin = 90000;
for (int i = 0; i <heightMap.Length;i++)
{
if (Single.IsNaN(heightMap[i]) || Single.IsInfinity(heightMap[i]))
{
heightMap[i] = 0f;
}
hfmin = (heightMap[i] < hfmin) ? heightMap[i] : hfmin;
hfmax = (heightMap[i] > hfmax) ? heightMap[i] : hfmax;
}
// store this for later reference.
// Note, we're storing it after we check it for anomolies above
_origheightmap = heightMap;
hfmin = 0;
hfmax = 256;
m_terrainShape = new btHeightfieldTerrainShape((int)Constants.RegionSize, (int)Constants.RegionSize, heightMap,
1.0f, hfmin, hfmax, (int)btHeightfieldTerrainShape.UPAxis.Z,
(int)btHeightfieldTerrainShape.PHY_ScalarType.PHY_FLOAT, false);
float AabbCenterX = Constants.RegionSize/2f;
float AabbCenterY = Constants.RegionSize/2f;
float AabbCenterZ = 0;
float temphfmin, temphfmax;
temphfmin = hfmin;
temphfmax = hfmax;
if (temphfmin < 0)
{
temphfmax = 0 - temphfmin;
temphfmin = 0 - temphfmin;
}
else if (temphfmin > 0)
{
temphfmax = temphfmax + (0 - temphfmin);
//temphfmin = temphfmin + (0 - temphfmin);
}
AabbCenterZ = temphfmax/2f;
if (m_terrainPosition == null)
{
m_terrainPosition = new btVector3(AabbCenterX, AabbCenterY, AabbCenterZ);
}
else
{
try
{
m_terrainPosition.setValue(AabbCenterX, AabbCenterY, AabbCenterZ);
}
catch (ObjectDisposedException)
{
m_terrainPosition = new btVector3(AabbCenterX, AabbCenterY, AabbCenterZ);
}
}
if (m_terrainMotionState != null)
{
m_terrainMotionState.Dispose();
m_terrainMotionState = null;
}
m_terrainTransform = new btTransform(QuatIdentity, m_terrainPosition);
m_terrainMotionState = new btDefaultMotionState(m_terrainTransform);
TerrainBody = new btRigidBody(0, m_terrainMotionState, m_terrainShape);
TerrainBody.setUserPointer((IntPtr)0);
m_world.addRigidBody(TerrainBody);
}
public override void SetTerrain (ITerrainChannel channel, short[] shortheightMap)
{
if (m_terrainShape != null)
DeleteTerrain();
float hfmax = 256;
float hfmin = 0;
// store this for later reference.
// Note, we're storing it after we check it for anomolies above
_origheightmap = shortheightMap;
hfmin = 0;
hfmax = 256;
float[] heightmap = new float[m_region.RegionSizeX * m_region.RegionSizeX];
for (int i = 0; i < shortheightMap.Length; i++)
{
heightmap[i] = shortheightMap[i] / Constants.TerrainCompression;
}
m_terrainShape = new btHeightfieldTerrainShape(m_region.RegionSizeX, m_region.RegionSizeY, heightmap,
1.0f, hfmin, hfmax, (int)btHeightfieldTerrainShape.UPAxis.Z,
(int)btHeightfieldTerrainShape.PHY_ScalarType.PHY_FLOAT, false);
float AabbCenterX = m_region.RegionSizeX / 2f;
float AabbCenterY = m_region.RegionSizeY / 2f;
float AabbCenterZ = 0;
float temphfmin, temphfmax;
temphfmin = hfmin;
temphfmax = hfmax;
if (temphfmin < 0)
{
temphfmax = 0 - temphfmin;
temphfmin = 0 - temphfmin;
}
else if (temphfmin > 0)
{
temphfmax = temphfmax + (0 - temphfmin);
//temphfmin = temphfmin + (0 - temphfmin);
}
AabbCenterZ = temphfmax/2f;
if (m_terrainPosition == null)
{
m_terrainPosition = new btVector3(AabbCenterX, AabbCenterY, AabbCenterZ);
}
else
{
try
{
m_terrainPosition.setValue(AabbCenterX, AabbCenterY, AabbCenterZ);
}
catch (ObjectDisposedException)
{
m_terrainPosition = new btVector3(AabbCenterX, AabbCenterY, AabbCenterZ);
}
}
if (m_terrainMotionState != null)
{
m_terrainMotionState.Dispose();
m_terrainMotionState = null;
}
m_terrainTransform = new btTransform(QuatIdentity, m_terrainPosition);
m_terrainMotionState = new btDefaultMotionState(m_terrainTransform);
TerrainBody = new btRigidBody(0, m_terrainMotionState, m_terrainShape);
TerrainBody.setUserPointer((IntPtr)0);
m_world.addRigidBody(TerrainBody);
}
private void rotate(float timestep)
{
m_log.Debug("[PHYSICS]: _________ChangeRotate");
tempTransform2 = Body.getWorldTransform();
tempOrientation2 = new btQuaternion(_orientation.X, _orientation.Y, _orientation.Z, _orientation.W);
tempTransform2.setRotation(tempOrientation2);
Body.setWorldTransform(tempTransform2);
resetCollisionAccounting();
m_taintrot = _orientation;
}
public btTransform inverseTimes(btTransform t)
{
if (m_disposed)
throw new ObjectDisposedException(ToString());
return FromIntPtr(BulletAPI_BtTransform_inverseTimes(m_handle,t.Handle));
}
public btTransform(btTransform t)
{
m_handle = BulletAPI_CreateBtTransformOtherTransform(t.Handle);
}
public void mult(btTransform t1, btTransform t2)
{
if (m_disposed)
throw new ObjectDisposedException(ToString());
BulletAPI_BtTransform_mult(m_handle, t1.Handle, t2.Handle);
}
public btDefaultMotionState(btTransform startTrans) : base()
{
m_handle = BulletAPI_CreateBtDefaultMotionStateStartTrans(startTrans.Handle);
}
public btDefaultMotionState(btTransform startTrans, btTransform centerOfMassOffset)
{
m_handle = BulletAPI_CreateBtDefaultMotionStateStartTransCenterOfMassOffset(startTrans.Handle, centerOfMassOffset.Handle);
}
public btDefaultMotionState(btTransform startTrans) : base()
{
m_handle = BulletAPI_CreateBtDefaultMotionStateStartTrans(startTrans.Handle);
}
internal void Dispose()
{
//TODO:
DisableAxisMotor();
DisposeOfBody();
SetCollisionShape(null);
if (tempMotionState3 != null && tempMotionState3.Handle != IntPtr.Zero)
{
tempMotionState3.Dispose();
tempMotionState3 = null;
}
if (tempMotionState2 != null && tempMotionState2.Handle != IntPtr.Zero)
{
tempMotionState2.Dispose();
tempMotionState2 = null;
}
if (tempMotionState1 != null && tempMotionState1.Handle != IntPtr.Zero)
{
tempMotionState1.Dispose();
tempMotionState1 = null;
}
if (tempTransform4 != null && tempTransform4.Handle != IntPtr.Zero)
{
tempTransform4.Dispose();
tempTransform4 = null;
}
if (tempTransform3 != null && tempTransform3.Handle != IntPtr.Zero)
{
tempTransform3.Dispose();
tempTransform3 = null;
}
if (tempTransform2 != null && tempTransform2.Handle != IntPtr.Zero)
{
tempTransform2.Dispose();
tempTransform2 = null;
}
if (tempTransform1 != null && tempTransform1.Handle != IntPtr.Zero)
{
tempTransform1.Dispose();
tempTransform1 = null;
}
if (tempOrientation2 != null && tempOrientation2.Handle != IntPtr.Zero)
{
tempOrientation2.Dispose();
tempOrientation2 = null;
}
if (tempOrientation1 != null && tempOrientation1.Handle != IntPtr.Zero)
{
tempOrientation1.Dispose();
tempOrientation1 = null;
}
if (tempInertia1 != null && tempInertia1.Handle != IntPtr.Zero)
{
tempInertia1.Dispose();
tempInertia1 = null;
}
if (tempInertia2 != null && tempInertia2.Handle != IntPtr.Zero)
{
tempInertia2.Dispose();
tempInertia1 = null;
}
if (tempAngularVelocity2 != null && tempAngularVelocity2.Handle != IntPtr.Zero)
{
tempAngularVelocity2.Dispose();
tempAngularVelocity2 = null;
}
if (tempAngularVelocity1 != null && tempAngularVelocity1.Handle != IntPtr.Zero)
{
tempAngularVelocity1.Dispose();
tempAngularVelocity1 = null;
}
if (tempLinearVelocity2 != null && tempLinearVelocity2.Handle != IntPtr.Zero)
{
tempLinearVelocity2.Dispose();
tempLinearVelocity2 = null;
}
if (tempLinearVelocity1 != null && tempLinearVelocity1.Handle != IntPtr.Zero)
{
tempLinearVelocity1.Dispose();
tempLinearVelocity1 = null;
}
if (tempSize2 != null && tempSize2.Handle != IntPtr.Zero)
{
tempSize2.Dispose();
tempSize2 = null;
}
if (tempSize1 != null && tempSize1.Handle != IntPtr.Zero)
{
tempSize1.Dispose();
tempSize1 = null;
}
if (tempPosition3 != null && tempPosition3.Handle != IntPtr.Zero)
{
tempPosition3.Dispose();
tempPosition3 = null;
}
if (tempPosition2 != null && tempPosition2.Handle != IntPtr.Zero)
{
tempPosition2.Dispose();
tempPosition2 = null;
}
if (tempPosition1 != null && tempPosition1.Handle != IntPtr.Zero)
{
tempPosition1.Dispose();
tempPosition1 = null;
}
if (AxisLockLinearLow != null && AxisLockLinearLow.Handle != IntPtr.Zero)
{
AxisLockLinearLow.Dispose();
AxisLockLinearLow = null;
}
if (AxisLockLinearHigh != null && AxisLockLinearHigh.Handle != IntPtr.Zero)
{
AxisLockLinearHigh.Dispose();
AxisLockLinearHigh = null;
}
}
private void changemove(float timestep)
{
m_log.Debug("[PHYSICS]: _________ChangeMove");
if (!m_isphysical)
{
tempTransform2 = Body.getWorldTransform();
btQuaternion quat = tempTransform2.getRotation();
tempPosition2.setValue(_position.X, _position.Y, _position.Z);
tempTransform2.Dispose();
tempTransform2 = new btTransform(quat, tempPosition2);
Body.setWorldTransform(tempTransform2);
changeSelectedStatus(timestep);
resetCollisionAccounting();
}
else
{
if (Body != null)
{
if (Body.Handle != IntPtr.Zero)
{
DisableAxisMotor();
_parent_scene.removeFromWorld(this, Body);
//Body.Dispose();
}
//Body = null;
// TODO: dispose parts that make up body
}
/*
if (_parent_scene.needsMeshing(_pbs))
{
// Don't need to re-enable body.. it's done in SetMesh
float meshlod = _parent_scene.meshSculptLOD;
if (IsPhysical)
meshlod = _parent_scene.MeshSculptphysicalLOD;
IMesh mesh = _parent_scene.mesher.CreateMesh(SOPName, _pbs, _size, meshlod, IsPhysical);
// createmesh returns null when it doesn't mesh.
CreateGeom(IntPtr.Zero, mesh);
}
else
{
_mesh = null;
CreateGeom(IntPtr.Zero, null);
}
SetCollisionShape(prim_geom);
*/
if (m_isphysical)
SetBody(Mass);
else
SetBody(0);
changeSelectedStatus(timestep);
resetCollisionAccounting();
}
m_taintposition = _position;
}
private void MoveLinear(float timestep)
{
if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant
{
// add drive to body
Vector3 addAmount = m_linearMotorDirection / (m_linearMotorTimescale / timestep);
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 / timestep)));
//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;
btQuaternion rot = m_body.getWorldTransform().getRotation();
Quaternion rotq = new Quaternion(rot.getX(), rot.getY(), rot.getZ(), rot.getW()); // 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.
float objMass = m_prim.Mass;
// m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
//Rev: bullet does gravity internally
grav.Z = -parent_scene.gravityz * objMass * m_VehicleBuoyancy; //parent_scene.gravityz/* * objMass*/ * (1f - m_VehicleBuoyancy);
// Preserve the current Z velocity
btVector3 pos = m_body.getWorldTransform().getOrigin();
btVector3 newpos = pos;
m_dir.Z = m_prim.Velocity.Z; // Preserve the accumulated falling velocity
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 = newpos.getX() - m_lastPositionVector.getX();
posChange.Y = newpos.getY() - m_lastPositionVector.getY();
posChange.Z = newpos.getZ() - m_lastPositionVector.getZ();
double Zchange = Math.Abs(posChange.Z);
btQuaternion Orientation2 = m_body.getWorldTransform().getRotation();
/*if (m_BlockingEndPoint != Vector3.Zero)
{
if (newpos.getX() >= (m_BlockingEndPoint.X - (float)1))
newpos.setX(newpos.getX() - (posChange.X + 1));
if (newpos.getY() >= (m_BlockingEndPoint.Y - (float)1))
newpos.setY(newpos.getY() - (posChange.Y + 1));
if (newpos.getZ() >= (m_BlockingEndPoint.Z - (float)1))
newpos.setZ(newpos.getZ() - (posChange.Z + 1));
if (newpos.getX() <= 0)
newpos.setX(newpos.getX() + (posChange.X + 1));
if (newpos.getY() <= 0)
newpos.setY(newpos.getY() + (posChange.Y + 1));
}
*/
if (newpos.getZ() < parent_scene.GetTerrainHeightAtXY(newpos.getX(), newpos.getY()))
newpos.setZ(parent_scene.GetTerrainHeightAtXY(newpos.getX(), newpos.getY()) + 2);
// Check if hovering
if ((m_Hoverflags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
{
float diff = (newpos.getZ() - m_VhoverTargetHeight);
// We should hover, get the target height
if ((m_Hoverflags & VehicleFlag.HOVER_WATER_ONLY) != 0)
m_VhoverTargetHeight = parent_scene.GetWaterLevel() + m_VhoverHeight;
if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
m_VhoverTargetHeight = parent_scene.GetTerrainHeightAtXY(pos.getX(), pos.getY()) + 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 (newpos.getZ() > m_VhoverTargetHeight) m_VhoverTargetHeight = newpos.getZ();
}
if ((m_Hoverflags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
{
if (diff > .2 || diff < -.2)
{
newpos.setValue(newpos.getX(), newpos.getY(), m_VhoverTargetHeight);
btTransform trans = new btTransform(Orientation2, newpos);
m_body.setWorldTransform(trans);
}
}
else
{
// Replace Vertical speed with correction figure if significant
if (Math.Abs(diff) > 0.01f)
m_dir.Z = -((diff * timestep * 50.0f) / m_VhoverTimescale);
else
m_dir.Z = 0f;
}
}
/*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 = parent_scene.GetTerrainHeightAtXY(pos.getX(), pos.getY());
float postemp = (pos.getZ() - 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 = new btVector3(m_prim.Position.X,m_prim.Position.Y,m_prim.Position.Z);
// Apply velocity
//if(m_dir != Vector3.Zero)
// m_body.setLinearVelocity(new btVector3(m_dir.X, m_dir.Y, m_dir.Z));
m_body.applyCentralImpulse(new btVector3(m_dir.X, m_dir.Y, m_dir.Z));
// apply gravity force
//m_body.applyCentralImpulse(new btVector3(0, 0, 9.8f));
/*ector3 newpos2 = new Vector3(newpos.getX(), newpos.getY(), newpos.getZ());
if (newpos2.X != m_prim.Position.X || newpos2.Y != m_prim.Position.Y || newpos2.Z != m_prim.Position.Z)
{
btTransform trans = new btTransform(Orientation2, newpos);
m_body.setWorldTransform(trans);
}*/
// apply friction
Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / timestep);
m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
}
public BulletDotNETScene(string sceneIdentifier)
{
BulletLock = new object();
// m_sceneIdentifier = sceneIdentifier;
VectorZero = new btVector3(0, 0, 0);
QuatIdentity = new btQuaternion(0, 0, 0, 1);
TransZero = new btTransform(QuatIdentity, VectorZero);
m_gravity = new btVector3(0, 0, gravityz);
}
public void setWorldTransform(btTransform worldTrans)
{
if (m_disposed)
throw new ObjectDisposedException(ToString());
BulletAPI_BtCollisionObject_setWorldTransform(m_handle, worldTrans.Handle);
}
public BulletDotNETScene(string sceneIdentifier)
{
BulletLock = new object();
// m_sceneIdentifier = sceneIdentifier;
VectorZero = new btVector3(0, 0, 0);
QuatIdentity = new btQuaternion(0, 0, 0, 1);
TransZero = new btTransform(QuatIdentity, VectorZero);
m_gravity = new btVector3(0, 0, gravityz);
_origheightmap = new float[(int)Constants.RegionSize * (int)Constants.RegionSize];
}
/// <summary>
/// Updates the reported position and velocity. This essentially sends the data up to ScenePresence.
/// </summary>
public void UpdatePositionAndVelocity()
{
if (Body == null)
return;
//int val = Environment.TickCount;
CheckIfStandingOnObject();
//m_log.DebugFormat("time:{0}", Environment.TickCount - val);
//IsColliding = Body.checkCollideWith(m_parent_scene.TerrainBody);
tempTrans1.Dispose();
tempTrans1 = Body.getInterpolationWorldTransform();
tempVector1.Dispose();
tempVector1 = tempTrans1.getOrigin();
tempVector2.Dispose();
tempVector2 = Body.getInterpolationLinearVelocity();
// no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
Vector3 vec = new Vector3(tempVector1.getX(), tempVector1.getY(), tempVector1.getZ());
// kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!)
if (vec.X < -10.0f) vec.X = 0.0f;
if (vec.Y < -10.0f) vec.Y = 0.0f;
if (vec.X > (int)Constants.RegionSize + 10.2f) vec.X = (int)Constants.RegionSize + 10.2f;
if (vec.Y > (int)Constants.RegionSize + 10.2f) vec.Y = (int)Constants.RegionSize + 10.2f;
m_position.X = vec.X;
m_position.Y = vec.Y;
m_position.Z = vec.Z;
// Did we move last? = zeroflag
// This helps keep us from sliding all over
if (m_zeroFlag)
{
m_velocity.X = 0.0f;
m_velocity.Y = 0.0f;
m_velocity.Z = 0.0f;
// Did we send out the 'stopped' message?
if (!m_lastUpdateSent)
{
m_lastUpdateSent = true;
base.RequestPhysicsterseUpdate();
}
}
else
{
m_lastUpdateSent = false;
vec = new Vector3(tempVector2.getX(), tempVector2.getY(), tempVector2.getZ());
m_velocity.X = (vec.X);
m_velocity.Y = (vec.Y);
m_velocity.Z = (vec.Z);
//m_log.Debug(m_target_velocity);
if (m_velocity.Z < -6 && !m_hackSentFall)
{
m_hackSentFall = true;
m_pidControllerActive = false;
}
else if (m_flying && !m_hackSentFly)
{
//m_hackSentFly = true;
//base.SendCollisionUpdate(new CollisionEventUpdate());
}
else
{
m_hackSentFly = false;
m_hackSentFall = false;
}
}
if (Body != null)
{
if (Body.getFriction() < 0.9f)
Body.setFriction(0.9f);
}
//if (Body != null)
// Body.clearForces();
}
public override void DeleteTerrain()
{
if (TerrainBody != null)
{
m_world.removeRigidBody(TerrainBody);
}
if (m_terrainShape != null)
{
m_terrainShape.Dispose();
m_terrainShape = null;
}
if (m_terrainMotionState != null)
{
m_terrainMotionState.Dispose();
m_terrainMotionState = null;
}
if (m_terrainTransform != null)
{
m_terrainTransform.Dispose();
m_terrainTransform = null;
}
if (m_terrainPosition != null)
{
m_terrainPosition.Dispose();
m_terrainPosition = null;
}
}
/// <summary>
/// This creates the Avatar's physical Surrogate at the position supplied
/// </summary>
/// <param name="npositionX"></param>
/// <param name="npositionY"></param>
/// <param name="npositionZ"></param>
// WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access
// to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only
// place that is safe to call this routine AvatarGeomAndBodyCreation.
private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ)
{
if (CAPSULE_LENGTH <= 0)
{
m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!");
CAPSULE_LENGTH = 0.01f;
}
if (CAPSULE_RADIUS <= 0)
{
m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!");
CAPSULE_RADIUS = 0.01f;
}
Shell = new btCapsuleShape(CAPSULE_RADIUS, CAPSULE_LENGTH);
if (m_bodyPosition == null)
m_bodyPosition = new btVector3(npositionX, npositionY, npositionZ);
m_bodyPosition.setValue(npositionX, npositionY, npositionZ);
if (m_bodyOrientation == null)
m_bodyOrientation = new btQuaternion(m_CapsuleOrientationAxis, (Utils.DEG_TO_RAD * 90));
if (m_bodyTransform == null)
m_bodyTransform = new btTransform(m_bodyOrientation, m_bodyPosition);
else
{
m_bodyTransform.Dispose();
m_bodyTransform = new btTransform(m_bodyOrientation, m_bodyPosition);
}
if (m_bodyMotionState == null)
m_bodyMotionState = new btDefaultMotionState(m_bodyTransform);
else
m_bodyMotionState.setWorldTransform(m_bodyTransform);
m_mass = Mass;
Body = new btRigidBody(m_mass, m_bodyMotionState, Shell);
// this is used for self identification. User localID instead of body handle
Body.setUserPointer(new IntPtr((int)m_localID));
if (ClosestCastResult != null)
ClosestCastResult.Dispose();
ClosestCastResult = new ClosestNotMeRayResultCallback(Body);
m_parent_scene.AddRigidBody(Body);
Body.setActivationState(4);
if (m_aMotor != null)
{
if (m_aMotor.Handle != IntPtr.Zero)
{
m_parent_scene.getBulletWorld().removeConstraint(m_aMotor);
m_aMotor.Dispose();
}
m_aMotor = null;
}
m_aMotor = new btGeneric6DofConstraint(Body, m_parent_scene.TerrainBody,
m_parent_scene.TransZero,
m_parent_scene.TransZero, false);
m_aMotor.setAngularLowerLimit(m_parent_scene.VectorZero);
m_aMotor.setAngularUpperLimit(m_parent_scene.VectorZero);
}
/// <summary>
/// Updates the reported position and velocity. This essentially sends the data up to ScenePresence.
/// </summary>
public void UpdatePositionAndVelocity()
{
if (Body == null)
return;
//int val = Environment.TickCount;
CheckIfStandingOnObject();
//m_log.DebugFormat("time:{0}", Environment.TickCount - val);
//IsColliding = Body.checkCollideWith(m_parent_scene.TerrainBody);
tempTrans1.Dispose();
tempTrans1 = Body.getInterpolationWorldTransform();
tempVector1.Dispose();
tempVector1 = tempTrans1.getOrigin();
tempVector2.Dispose();
tempVector2 = Body.getInterpolationLinearVelocity();
// no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
Vector3 vec = new Vector3(tempVector1.getX(), tempVector1.getY(), tempVector1.getZ());
// kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!)
if (vec.X < -10.0f) vec.X = 0.0f;
if (vec.Y < -10.0f) vec.Y = 0.0f;
if (vec.X > m_parent_scene.m_region.RegionSizeX + 10.2f) vec.X = m_parent_scene.m_region.RegionSizeX + 10.2f;
if (vec.Y > m_parent_scene.m_region.RegionSizeY + 10.2f) vec.Y = m_parent_scene.m_region.RegionSizeY + 10.2f;
m_position.X = vec.X;
m_position.Y = vec.Y;
m_position.Z = vec.Z;
// Did we move last? = zeroflag
// This helps keep us from sliding all over
if (m_zeroFlag)
{
m_velocity.X = 0.0f;
m_velocity.Y = 0.0f;
m_velocity.Z = 0.0f;
// Did we send out the 'stopped' message?
if (!m_lastUpdateSent)
{
m_lastUpdateSent = true;
base.RequestPhysicsterseUpdate();
}
//Tell any listeners that we've stopped
base.TriggerMovementUpdate();
}
else
{
m_lastUpdateSent = false;
vec = new Vector3(tempVector2.getX(), tempVector2.getY(), tempVector2.getZ());
m_velocity.X = (vec.X);
m_velocity.Y = (vec.Y);
m_velocity.Z = (vec.Z);
//m_log.Debug(m_target_velocity);
if (m_velocity.Z < -6 && !m_hackSentFall)
{
m_hackSentFall = true;
m_pidControllerActive = false;
}
else if (m_flying && !m_hackSentFly)
{
//m_hackSentFly = true;
//base.SendCollisionUpdate(new CollisionEventUpdate());
}
else
{
m_hackSentFly = false;
m_hackSentFall = false;
}
const float VELOCITY_TOLERANCE = 0.001f;
const float POSITION_TOLERANCE = 0.05f;
//Check to see whether we need to trigger the significant movement method in the presence
if (!RotationalVelocity.ApproxEquals(m_lastRotationalVelocity, VELOCITY_TOLERANCE) ||
!Velocity.ApproxEquals(m_lastVelocity, VELOCITY_TOLERANCE) ||
!Position.ApproxEquals(m_lastPosition, POSITION_TOLERANCE))
{
// Update the "last" values
m_lastPosition = Position;
m_lastRotationalVelocity = RotationalVelocity;
m_lastVelocity = Velocity;
base.RequestPhysicsterseUpdate();
base.TriggerSignificantMovement();
}
//Tell any listeners about the new info
base.TriggerMovementUpdate();
}
if (Body != null)
{
if (Body.getFriction() < 0.9f)
Body.setFriction(0.9f);
}
//if (Body != null)
// Body.clearForces();
}
public btDefaultMotionState(btTransform startTrans, btTransform centerOfMassOffset)
{
m_handle = BulletAPI_CreateBtDefaultMotionStateStartTransCenterOfMassOffset(startTrans.Handle, centerOfMassOffset.Handle);
}
public btTransform(btTransform t)
{
m_handle = BulletAPI_CreateBtTransformOtherTransform(t.Handle);
}
private void changeshape(float timestep)
{
if (Body != null)
{
if (Body.Handle != IntPtr.Zero)
{
DisableAxisMotor();
_parent_scene.removeFromWorld(this, Body);
//Body.Dispose();
}
//Body = null;
// TODO: dispose parts that make up body
}
// Cleanup of old prim geometry and Bodies
if (IsPhysical && Body != null && Body.Handle != IntPtr.Zero)
{
if (childPrim)
{
if (_parent != null)
{
BulletDotNETPrim parent = (BulletDotNETPrim)_parent;
parent.ChildDelink(this);
}
}
else
{
//disableBody();
}
}
try
{
//SetCollisionShape(null);
}
catch (System.AccessViolationException)
{
//prim_geom = IntPtr.Zero;
m_log.Error("[PHYSICS]: PrimGeom dead");
}
// we don't need to do space calculation because the client sends a position update also.
if (_size.X <= 0) _size.X = 0.01f;
if (_size.Y <= 0) _size.Y = 0.01f;
if (_size.Z <= 0) _size.Z = 0.01f;
// Construction of new prim
ProcessGeomCreation();
tempPosition1.setValue(_position.X, _position.Y, _position.Z);
if (tempOrientation1.Handle != IntPtr.Zero)
tempOrientation1.Dispose();
tempOrientation1 = new btQuaternion(_orientation.X, Orientation.Y, _orientation.Z, _orientation.W);
if (tempTransform1 != null && tempTransform1.Handle != IntPtr.Zero)
tempTransform1.Dispose();
tempTransform1 = new btTransform(tempOrientation1, tempPosition1);
//d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
if (IsPhysical)
{
SetBody(Mass);
// Re creates body on size.
// EnableBody also does setMass()
}
else
{
SetBody(0);
}
changeSelectedStatus(timestep);
if (childPrim)
{
if (_parent is BulletDotNETPrim)
{
BulletDotNETPrim parent = (BulletDotNETPrim)_parent;
parent.ChildSetGeom(this);
}
}
resetCollisionAccounting();
m_taintshape = false;
}
public BulletDotNETCharacter(string avName, BulletDotNETScene parent_scene, Vector3 pos, 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_position = pos;
m_zeroPosition = pos;
m_parent_scene = parent_scene;
PID_D = pid_d;
PID_P = pid_p;
CAPSULE_RADIUS = capsule_radius;
m_density = density;
heightFudgeFactor = height_fudge_factor;
walkDivisor = walk_divisor;
runDivisor = rundivisor;
for (int i = 0; i < 11; i++)
{
m_colliderarr[i] = false;
}
for (int i = 0; i < 11; i++)
{
m_colliderGroundarr[i] = false;
}
CAPSULE_LENGTH = (size.Z * 1.15f) - CAPSULE_RADIUS * 2.0f;
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_parent_scene.AddPhysicsActorTaint(this);
// m_name = avName;
tempVector1 = new btVector3(0, 0, 0);
tempVector2 = new btVector3(0, 0, 0);
tempVector3 = new btVector3(0, 0, 0);
tempVector4 = new btVector3(0, 0, 0);
tempVector5RayCast = new btVector3(0, 0, 0);
tempVector6RayCast = new btVector3(0, 0, 0);
tempVector7RayCast = new btVector3(0, 0, 0);
tempQuat1 = new btQuaternion(0, 0, 0, 1);
tempTrans1 = new btTransform(tempQuat1, tempVector1);
// m_movementComparision = new PhysicsVector(0, 0, 0);
m_CapsuleOrientationAxis = new btVector3(1, 0, 1);
}
public BulletDotNETPrim(String primName, BulletDotNETScene parent_scene, Vector3 pos, Vector3 size,
Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical)
{
tempPosition1 = new btVector3(0, 0, 0);
tempPosition2 = new btVector3(0, 0, 0);
tempPosition3 = new btVector3(0, 0, 0);
tempSize1 = new btVector3(0, 0, 0);
tempSize2 = new btVector3(0, 0, 0);
tempLinearVelocity1 = new btVector3(0, 0, 0);
tempLinearVelocity2 = new btVector3(0, 0, 0);
tempAngularVelocity1 = new btVector3(0, 0, 0);
tempAngularVelocity2 = new btVector3(0, 0, 0);
tempInertia1 = new btVector3(0, 0, 0);
tempInertia2 = new btVector3(0, 0, 0);
tempOrientation1 = new btQuaternion(0, 0, 0, 1);
tempOrientation2 = new btQuaternion(0, 0, 0, 1);
_parent_scene = parent_scene;
tempTransform1 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero);
tempTransform2 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero); ;
tempTransform3 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero); ;
tempTransform4 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero); ;
tempMotionState1 = new btDefaultMotionState(_parent_scene.TransZero);
tempMotionState2 = new btDefaultMotionState(_parent_scene.TransZero);
tempMotionState3 = new btDefaultMotionState(_parent_scene.TransZero);
AxisLockLinearLow = new btVector3(-1 * (int)Constants.RegionSize, -1 * (int)Constants.RegionSize, -1 * (int)Constants.RegionSize);
int regionsize = (int)Constants.RegionSize;
if (regionsize == 256)
regionsize = 512;
AxisLockLinearHigh = new btVector3((int)Constants.RegionSize, (int)Constants.RegionSize, (int)Constants.RegionSize);
_target_velocity = Vector3.Zero;
_velocity = Vector3.Zero;
_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 = null;
Body = null;
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 = Vector3.Zero;
m_rotationalVelocity = Vector3.Zero;
_orientation = rotation;
m_taintrot = _orientation;
_mesh = mesh;
_pbs = pbs;
_parent_scene = parent_scene;
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
}
m_primName = primName;
m_taintadd = true;
_parent_scene.AddPhysicsActorTaint(this);
}
public void ProcessTaints(float timestep)
{
if (m_tainted_isPhysical != m_isPhysical)
{
if (m_tainted_isPhysical)
{
// Create avatar capsule and related ODE data
if (!(Shell == null && Body == null))
{
m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - "
+ (Shell != null ? "Shell " : "")
+ (Body != null ? "Body " : ""));
}
AvatarGeomAndBodyCreation(m_position.X, m_position.Y, m_position.Z);
}
else
{
// destroy avatar capsule and related ODE data
Dispose();
tempVector1 = new btVector3(0, 0, 0);
tempVector2 = new btVector3(0, 0, 0);
tempVector3 = new btVector3(0, 0, 0);
tempVector4 = new btVector3(0, 0, 0);
tempVector5RayCast = new btVector3(0, 0, 0);
tempVector6RayCast = new btVector3(0, 0, 0);
tempVector7RayCast = new btVector3(0, 0, 0);
tempQuat1 = new btQuaternion(0, 0, 0, 1);
tempTrans1 = new btTransform(tempQuat1, tempVector1);
// m_movementComparision = new PhysicsVector(0, 0, 0);
m_CapsuleOrientationAxis = new btVector3(1, 0, 1);
}
m_isPhysical = m_tainted_isPhysical;
}
if (m_tainted_CAPSULE_LENGTH != CAPSULE_LENGTH)
{
if (Body != null)
{
m_pidControllerActive = true;
// no lock needed on _parent_scene.OdeLock because we are called from within the thread lock in OdePlugin's simulate()
//d.JointDestroy(Amotor);
float prevCapsule = CAPSULE_LENGTH;
CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH;
//m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
Dispose();
tempVector1 = new btVector3(0, 0, 0);
tempVector2 = new btVector3(0, 0, 0);
tempVector3 = new btVector3(0, 0, 0);
tempVector4 = new btVector3(0, 0, 0);
tempVector5RayCast = new btVector3(0, 0, 0);
tempVector6RayCast = new btVector3(0, 0, 0);
tempVector7RayCast = new btVector3(0, 0, 0);
tempQuat1 = new btQuaternion(0, 0, 0, 1);
tempTrans1 = new btTransform(tempQuat1, tempVector1);
// m_movementComparision = new PhysicsVector(0, 0, 0);
m_CapsuleOrientationAxis = new btVector3(1, 0, 1);
AvatarGeomAndBodyCreation(m_position.X, m_position.Y,
m_position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2));
Velocity = Vector3.Zero;
}
else
{
m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - "
+ (Shell == null ? "Shell " : "")
+ (Body == null ? "Body " : ""));
}
}
if (m_taintRemove)
{
Dispose();
}
}
public void SetBody(float mass)
{
if (!IsPhysical || childrenPrim.Count == 0)
{
if (tempMotionState1 != null && tempMotionState1.Handle != IntPtr.Zero)
tempMotionState1.Dispose();
if (tempTransform2 != null && tempTransform2.Handle != IntPtr.Zero)
tempTransform2.Dispose();
if (tempOrientation2 != null && tempOrientation2.Handle != IntPtr.Zero)
tempOrientation2.Dispose();
if (tempPosition2 != null && tempPosition2.Handle != IntPtr.Zero)
tempPosition2.Dispose();
tempOrientation2 = new btQuaternion(_orientation.X, _orientation.Y, _orientation.Z, _orientation.W);
tempPosition2 = new btVector3(_position.X, _position.Y, _position.Z);
tempTransform2 = new btTransform(tempOrientation2, tempPosition2);
tempMotionState1 = new btDefaultMotionState(tempTransform2, _parent_scene.TransZero);
if (tempInertia1 != null && tempInertia1.Handle != IntPtr.Zero)
tempInertia1.Dispose();
tempInertia1 = new btVector3(0, 0, 0);
prim_geom.calculateLocalInertia(mass, tempInertia1);
if (mass != 0)
_parent_scene.addActivePrim(this);
else
_parent_scene.remActivePrim(this);
// Body = new btRigidBody(mass, tempMotionState1, prim_geom);
//else
Body = new btRigidBody(mass, tempMotionState1, prim_geom, tempInertia1);
if (prim_geom is btGImpactMeshShape)
{
((btGImpactMeshShape)prim_geom).setLocalScaling(new btVector3(1, 1, 1));
((btGImpactMeshShape)prim_geom).updateBound();
}
//Body.setCollisionFlags(Body.getCollisionFlags() | (int)ContactFlags.CF_CUSTOM_MATERIAL_CALLBACK);
//Body.setUserPointer((IntPtr) (int)m_localID);
_parent_scene.AddPrimToScene(this);
}
else
{
// bool hasTrimesh = false;
lock (childrenPrim)
{
foreach (BulletDotNETPrim chld in childrenPrim)
{
if (chld == null)
continue;
// if (chld.NeedsMeshing())
// hasTrimesh = true;
}
}
//if (hasTrimesh)
//{
ProcessGeomCreationAsTriMesh(Vector3.Zero, Quaternion.Identity);
// createmesh returns null when it doesn't mesh.
/*
if (_mesh is Mesh)
{
}
else
{
m_log.Warn("[PHYSICS]: Can't link a OpenSim.Region.Physics.Meshing.Mesh object");
return;
}
*/
foreach (BulletDotNETPrim chld in childrenPrim)
{
if (chld == null)
continue;
Vector3 offset = chld.Position - Position;
Vector3 pos = new Vector3(offset.X, offset.Y, offset.Z);
pos *= Quaternion.Inverse(Orientation);
//pos *= Orientation;
offset = pos;
chld.ProcessGeomCreationAsTriMesh(offset, chld.Orientation);
_mesh.Append(chld._mesh);
}
setMesh(_parent_scene, _mesh);
//}
if (tempMotionState1 != null && tempMotionState1.Handle != IntPtr.Zero)
tempMotionState1.Dispose();
if (tempTransform2 != null && tempTransform2.Handle != IntPtr.Zero)
tempTransform2.Dispose();
if (tempOrientation2 != null && tempOrientation2.Handle != IntPtr.Zero)
tempOrientation2.Dispose();
if (tempPosition2 != null && tempPosition2.Handle != IntPtr.Zero)
tempPosition2.Dispose();
tempOrientation2 = new btQuaternion(_orientation.X, _orientation.Y, _orientation.Z, _orientation.W);
tempPosition2 = new btVector3(_position.X, _position.Y, _position.Z);
tempTransform2 = new btTransform(tempOrientation2, tempPosition2);
tempMotionState1 = new btDefaultMotionState(tempTransform2, _parent_scene.TransZero);
if (tempInertia1 != null && tempInertia1.Handle != IntPtr.Zero)
tempInertia1.Dispose();
tempInertia1 = new btVector3(0, 0, 0);
prim_geom.calculateLocalInertia(mass, tempInertia1);
if (mass != 0)
_parent_scene.addActivePrim(this);
else
_parent_scene.remActivePrim(this);
// Body = new btRigidBody(mass, tempMotionState1, prim_geom);
//else
Body = new btRigidBody(mass, tempMotionState1, prim_geom, tempInertia1);
if (prim_geom is btGImpactMeshShape)
{
((btGImpactMeshShape)prim_geom).setLocalScaling(new btVector3(1, 1, 1));
((btGImpactMeshShape)prim_geom).updateBound();
}
_parent_scene.AddPrimToScene(this);
}
if (IsPhysical)
changeAngularLock(0);
}
public btGeneric6DofConstraint(btRigidBody rbA, btRigidBody rbB, btTransform frameInA, btTransform frameInB, bool useLinearReferenceFrameA)
: base()
{
m_handle = BulletAPI_CreateBtGeneric6DofConstraint(rbA.Handle, rbB.Handle, frameInA.Handle, frameInB.Handle, useLinearReferenceFrameA);
}
public void UpdatePositionAndVelocity()
{
if (!m_isSelected)
{
if (_parent == null)
{
Vector3 pv = Vector3.Zero;
bool lastZeroFlag = _zeroFlag;
if (tempPosition3 != null && tempPosition3.Handle != IntPtr.Zero)
tempPosition3.Dispose();
if (tempTransform3 != null && tempTransform3.Handle != IntPtr.Zero)
tempTransform3.Dispose();
if (tempOrientation2 != null && tempOrientation2.Handle != IntPtr.Zero)
tempOrientation2.Dispose();
if (tempAngularVelocity1 != null && tempAngularVelocity1.Handle != IntPtr.Zero)
tempAngularVelocity1.Dispose();
if (tempLinearVelocity1 != null && tempLinearVelocity1.Handle != IntPtr.Zero)
tempLinearVelocity1.Dispose();
tempTransform3 = Body.getInterpolationWorldTransform();
tempPosition3 = tempTransform3.getOrigin(); // vec
tempOrientation2 = tempTransform3.getRotation(); // ori
tempAngularVelocity1 = Body.getInterpolationAngularVelocity(); //rotvel
tempLinearVelocity1 = Body.getInterpolationLinearVelocity(); // vel
_torque = new Vector3(tempAngularVelocity1.getX(), tempAngularVelocity1.getX(),
tempAngularVelocity1.getZ());
Vector3 l_position = Vector3.Zero;
Quaternion l_orientation = Quaternion.Identity;
m_lastposition = _position;
m_lastorientation = _orientation;
l_position.X = tempPosition3.getX();
l_position.Y = tempPosition3.getY();
l_position.Z = tempPosition3.getZ();
l_orientation.X = tempOrientation2.getX();
l_orientation.Y = tempOrientation2.getY();
l_orientation.Z = tempOrientation2.getZ();
l_orientation.W = tempOrientation2.getW();
if (l_position.X > ((int)Constants.RegionSize - 0.05f) || l_position.X < 0f || l_position.Y > ((int)Constants.RegionSize - 0.05f) || l_position.Y < 0f)
{
//base.RaiseOutOfBounds(l_position);
if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
{
_position = l_position;
//_parent_scene.remActivePrim(this);
if (_parent == null)
base.RequestPhysicsterseUpdate();
return;
}
else
{
if (_parent == null)
base.RaiseOutOfBounds(l_position);
return;
}
}
if (l_position.Z < -200000f)
{
// This is so prim that get lost underground don't fall forever and suck up
//
// Sim resources and memory.
// Disables the prim's movement physics....
// It's a hack and will generate a console message if it fails.
//IsPhysical = false;
//if (_parent == null)
//base.RaiseOutOfBounds(_position);
_acceleration.X = 0;
_acceleration.Y = 0;
_acceleration.Z = 0;
_velocity.X = 0;
_velocity.Y = 0;
_velocity.Z = 0;
m_rotationalVelocity.X = 0;
m_rotationalVelocity.Y = 0;
m_rotationalVelocity.Z = 0;
if (_parent == null)
base.RequestPhysicsterseUpdate();
m_throttleUpdates = false;
// throttleCounter = 0;
_zeroFlag = true;
//outofBounds = true;
}
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)
&& (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01))
{
_zeroFlag = true;
m_throttleUpdates = false;
}
else
{
//m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
_zeroFlag = false;
}
if (_zeroFlag)
{
_velocity.X = 0.0f;
_velocity.Y = 0.0f;
_velocity.Z = 0.0f;
_acceleration.X = 0;
_acceleration.Y = 0;
_acceleration.Z = 0;
//_orientation.w = 0f;
//_orientation.X = 0f;
//_orientation.Y = 0f;
//_orientation.Z = 0f;
m_rotationalVelocity.X = 0;
m_rotationalVelocity.Y = 0;
m_rotationalVelocity.Z = 0;
if (!m_lastUpdateSent)
{
m_throttleUpdates = false;
// throttleCounter = 0;
m_rotationalVelocity = pv;
if (_parent == null)
base.RequestPhysicsterseUpdate();
m_lastUpdateSent = true;
}
}
else
{
if (lastZeroFlag != _zeroFlag)
{
if (_parent == null)
base.RequestPhysicsterseUpdate();
}
m_lastVelocity = _velocity;
_position = l_position;
_velocity.X = tempLinearVelocity1.getX();
_velocity.Y = tempLinearVelocity1.getY();
_velocity.Z = tempLinearVelocity1.getZ();
_acceleration = ((_velocity - m_lastVelocity) / 0.1f);
_acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f,
_velocity.Y - m_lastVelocity.Y / 0.1f,
_velocity.Z - m_lastVelocity.Z / 0.1f);
//m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());
if (_velocity.ApproxEquals(pv, 0.5f))
{
m_rotationalVelocity = pv;
}
else
{
m_rotationalVelocity = new Vector3(tempAngularVelocity1.getX(), tempAngularVelocity1.getY(), tempAngularVelocity1.getZ());
}
//m_log.Debug("ODE: " + m_rotationalVelocity.ToString());
_orientation.X = l_orientation.X;
_orientation.Y = l_orientation.Y;
_orientation.Z = l_orientation.Z;
_orientation.W = l_orientation.W;
m_lastUpdateSent = false;
//if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
//{
if (_parent == null)
base.RequestPhysicsterseUpdate();
// }
// else
// {
// throttleCounter++;
//}
}
m_lastposition = l_position;
if (forceenable)
{
Body.forceActivationState(1);
forceenable = false;
}
}
else
{
// Not a body.. so Make sure the client isn't interpolating
_velocity.X = 0;
_velocity.Y = 0;
_velocity.Z = 0;
_acceleration.X = 0;
_acceleration.Y = 0;
_acceleration.Z = 0;
m_rotationalVelocity.X = 0;
m_rotationalVelocity.Y = 0;
m_rotationalVelocity.Z = 0;
_zeroFlag = true;
}
}
}
