public BasicActor()
{
_velocity = new PhysicsVector();
_position = new PhysicsVector();
_acceleration = new PhysicsVector();
_size = new PhysicsVector();
}
public static float GetDistanceTo(PhysicsVector a, PhysicsVector b)
{
float dx = a.X - b.X;
float dy = a.Y - b.Y;
float dz = a.Z - b.Z;
return((float)Math.Sqrt(dx * dx + dy * dy + dz * dz));
}
public override PhysicsActor AddAvatar(string avName, PhysicsVector position, PhysicsVector size, bool isFlying)
{
BasicActor act = new BasicActor();
act.Position = position;
act.Flying = isFlying;
_actors.Add(act);
return act;
}
public virtual void RaiseOutOfBounds(PhysicsVector pos)
{
// Make a temporary copy of the event to avoid possibility of
// a race condition if the last subscriber unsubscribes
// immediately after the null check and before the event is raised.
OutOfBounds handler = OnOutOfBounds;
if (handler != null)
{
handler(pos);
}
}
public virtual bool IsIdentical(PhysicsVector v, float tolerance)
{
PhysicsVector diff = this - v;
float d = diff.length();
if (d <= tolerance)
{
return(true);
}
return(false);
}
public void CreateAndDropPhysicalCube()
{
PrimitiveBaseShape newcube = PrimitiveBaseShape.CreateBox();
PhysicsVector position = new PhysicsVector(((int)Constants.RegionSize * 0.5f), ((int)Constants.RegionSize * 0.5f), 128);
PhysicsVector size = new PhysicsVector(0.5f, 0.5f, 0.5f);
Quaternion rot = Quaternion.Identity;
PhysicsActor prim = ps.AddPrimShape("CoolShape", newcube, position, size, rot, true);
OdePrim oprim = (OdePrim)prim;
OdeScene pscene = (OdeScene) ps;
Assert.That(oprim.m_taintadd);
prim.LocalID = 5;
for (int i = 0; i < 58; i++)
{
ps.Simulate(0.133f);
Assert.That(oprim.prim_geom != (IntPtr)0);
Assert.That(oprim.m_targetSpace != (IntPtr)0);
//Assert.That(oprim.m_targetSpace == pscene.space);
m_log.Info("TargetSpace: " + oprim.m_targetSpace + " - SceneMainSpace: " + pscene.space);
Assert.That(!oprim.m_taintadd);
m_log.Info("Prim Position (" + oprim.m_localID + "): " + prim.Position.ToString());
// Make sure we're above the ground
//Assert.That(prim.Position.Z > 20f);
//m_log.Info("PrimCollisionScore (" + oprim.m_localID + "): " + oprim.m_collisionscore);
// Make sure we've got a Body
Assert.That(oprim.Body != (IntPtr)0);
//m_log.Info(
}
// Make sure we're not somewhere above the ground
Assert.That(prim.Position.Z < 21.5f);
ps.RemovePrim(prim);
Assert.That(oprim.m_taintremove);
ps.Simulate(0.133f);
Assert.That(oprim.Body == (IntPtr)0);
}
public static bool isFinite(PhysicsVector v)
{
if (v == null)
{
return(false);
}
if (Single.IsInfinity(v.X) || Single.IsNaN(v.X))
{
return(false);
}
if (Single.IsInfinity(v.Y) || Single.IsNaN(v.Y))
{
return(false);
}
if (Single.IsInfinity(v.Z) || Single.IsNaN(v.Z))
{
return(false);
}
return(true);
}
internal void PushForce(PhysicsVector impulse)
{
if (PhysicsActor != null)
{
PhysicsActor.AddForce(impulse,true);
}
}
public abstract void LockAngularMotion(PhysicsVector axis);
/// <summary>
/// After all of the forces add up with 'add force' we apply them with doForce
/// </summary>
/// <param name="force"></param>
public void doForce(PhysicsVector force)
{
if (!collidelock)
{
d.BodyAddForce(Body, force.X, force.Y, force.Z);
//d.BodySetRotation(Body, ref m_StandUpRotation);
//standupStraight();
}
}
/// <summary>
/// Sets avatar height in the phyiscs plugin
/// </summary>
internal void SetHeight(float height)
{
m_avHeight = height;
if (PhysicsActor != null && !IsChildAgent)
{
PhysicsVector SetSize = new PhysicsVector(0.45f, 0.6f, m_avHeight);
PhysicsActor.Size = SetSize;
}
}
public abstract void VehicleVectorParam(int param, PhysicsVector value);
public void SetAcceleration(PhysicsVector accel)
{
m_pidControllerActive = true;
_acceleration = accel;
}
public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, PhysicsVector position,
PhysicsVector size, Quaternion rotation, bool isPhysical)
{
m_log.InfoFormat("[PHYSICS]: NullPhysicsScene : AddPrim({0},{1})", position, size);
return(PhysicsActor.Null);
}
public void SetAcceleration(PhysicsVector accel)
{
_acceleration = accel;
}
public override PhysicsActor AddAvatar(string avName, PhysicsVector position, PhysicsVector size, bool isFlying)
{
m_log.InfoFormat("[PHYSICS]: NullPhysicsScene : AddAvatar({0})", position);
return(PhysicsActor.Null);
}
/*
* public override PhysicsActor AddPrim(PhysicsVector position, PhysicsVector size, Quaternion rotation)
* {
* m_log.InfoFormat("NullPhysicsScene : AddPrim({0},{1})", position, size);
* return PhysicsActor.Null;
* }
*/
public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, PhysicsVector position,
PhysicsVector size, Quaternion rotation) //To be removed
{
return(AddPrimShape(primName, pbs, position, size, rotation, false));
}
public override void SetMomentum(PhysicsVector momentum)
{
}
public override void AddAngularForce(PhysicsVector force, bool pushforce)
{
}
public override void LockAngularMotion(PhysicsVector axis)
{
}
public override void VehicleVectorParam(int param, PhysicsVector value)
{
}
public abstract PhysicsActor AddAvatar(string avName, PhysicsVector position, PhysicsVector size, bool isFlying);
public override void VehicleVectorParam(int param, PhysicsVector value)
{
}
public abstract PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, PhysicsVector position,
PhysicsVector size, Quaternion rotation); //To be removed
public override void SetMomentum(PhysicsVector momentum)
{
}
PhysicsVector size, Quaternion rotation); //To be removed
public abstract PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, PhysicsVector position,
PhysicsVector size, Quaternion rotation, bool isPhysical);
public OdeCharacter(String avName, OdeScene parent_scene, PhysicsVector pos, CollisionLocker dode, PhysicsVector size, float pid_d, float pid_p, float capsule_radius, float tensor, float density, float height_fudge_factor, float walk_divisor, float rundivisor)
{
// ode = dode;
_velocity = new PhysicsVector();
_target_velocity = new PhysicsVector();
if (PhysicsVector.isFinite(pos))
{
if (pos.Z > 9999999)
{
pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
}
if (pos.Z < -90000)
{
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 PhysicsVector(((int)_parent_scene.WorldExtents.X * 0.5f), ((int)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128, 128) + 10);
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");
}
_acceleration = new PhysicsVector();
_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 virtual PhysicsJoint RequestJointCreation(string objectNameInScene, PhysicsJointType jointType, PhysicsVector position,
Quaternion rotation, string parms, List <string> bodyNames, string trackedBodyName, Quaternion localRotation)
{
return(null);
}
/// <summary>
/// Adds the force supplied to the Target Velocity
/// The PID controller takes this target velocity and tries to make it a reality
/// </summary>
/// <param name="force"></param>
public override void AddForce(PhysicsVector force, bool pushforce)
{
if (PhysicsVector.isFinite(force))
{
if (pushforce)
{
m_pidControllerActive = false;
force *= 100f;
doForce(force);
// If uncommented, things get pushed off world
//
// m_log.Debug("Push!");
// _target_velocity.X += force.X;
// _target_velocity.Y += force.Y;
// _target_velocity.Z += force.Z;
}
else
{
m_pidControllerActive = true;
_target_velocity.X += force.X;
_target_velocity.Y += force.Y;
_target_velocity.Z += force.Z;
}
}
else
{
m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character");
}
//m_lastUpdateSent = false;
}
public abstract void AddAngularForce(PhysicsVector force, bool pushforce);
/// <summary>
/// Called from Simulate
/// This is the avatar's movement control + PID Controller
/// </summary>
/// <param name="timeStep"></param>
public void Move(float timeStep, List<OdeCharacter> defects)
{
// 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 == IntPtr.Zero)
return;
if (m_pidControllerActive == false)
{
_zeroPosition = d.BodyGetPosition(Body);
}
//PidStatus = true;
d.Vector3 localpos = d.BodyGetPosition(Body);
PhysicsVector localPos = new PhysicsVector(localpos.X, localpos.Y, localpos.Z);
if (!PhysicsVector.isFinite(localPos))
{
m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
defects.Add(this);
// _parent_scene.RemoveCharacter(this);
// destroy avatar capsule and related ODE data
if (Amotor != IntPtr.Zero)
{
// Kill the Amotor
d.JointDestroy(Amotor);
Amotor = IntPtr.Zero;
}
//kill the Geometry
_parent_scene.waitForSpaceUnlock(_parent_scene.space);
if (Body != IntPtr.Zero)
{
//kill the body
d.BodyDestroy(Body);
Body = IntPtr.Zero;
}
if (Shell != IntPtr.Zero)
{
d.GeomDestroy(Shell);
_parent_scene.geom_name_map.Remove(Shell);
Shell = IntPtr.Zero;
}
return;
}
PhysicsVector vec = new PhysicsVector();
d.Vector3 vel = d.BodyGetLinearVel(Body);
float movementdivisor = 1f;
if (!m_alwaysRun)
{
movementdivisor = walkDivisor;
}
else
{
movementdivisor = runDivisor;
}
// if velocity is zero, use position control; otherwise, velocity control
if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f && m_iscolliding)
{
// keep track of where we stopped. No more slippin' & slidin'
if (!_zeroFlag)
{
_zeroFlag = true;
_zeroPosition = d.BodyGetPosition(Body);
}
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
d.Vector3 pos = d.BodyGetPosition(Body);
vec.X = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
vec.Y = (_target_velocity.Y - vel.Y)*(PID_D) + (_zeroPosition.Y - pos.Y)* (PID_P * 2);
if (flying)
{
vec.Z = (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
}
}
//PidStatus = true;
}
else
{
m_pidControllerActive = true;
_zeroFlag = false;
if (m_iscolliding && !flying)
{
// We're standing on something
vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D);
vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D);
}
else if (m_iscolliding && flying)
{
// We're flying and colliding with something
vec.X = ((_target_velocity.X/movementdivisor) - vel.X)*(PID_D / 16);
vec.Y = ((_target_velocity.Y/movementdivisor) - vel.Y)*(PID_D / 16);
}
else if (!m_iscolliding && flying)
{
// we're in mid air suspended
vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D/6);
vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D/6);
}
if (m_iscolliding && !flying && _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.
d.Vector3 pos = d.BodyGetPosition(Body);
vec.Z = (_target_velocity.Z - vel.Z)*PID_D + (_zeroPosition.Z - pos.Z)*PID_P;
if (_target_velocity.X > 0)
{
vec.X = ((_target_velocity.X - vel.X)/1.2f)*PID_D;
}
if (_target_velocity.Y > 0)
{
vec.Y = ((_target_velocity.Y - vel.Y)/1.2f)*PID_D;
}
}
else if (!m_iscolliding && !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 (_target_velocity.X > 0)
{
vec.X = ((_target_velocity.X - vel.X)/1.2f)*PID_D;
}
if (_target_velocity.Y > 0)
{
vec.Y = ((_target_velocity.Y - vel.Y)/1.2f)*PID_D;
}
}
if (flying)
{
vec.Z = (_target_velocity.Z - vel.Z) * (PID_D);
}
}
if (flying)
{
vec.Z += ((-1 * _parent_scene.gravityz)*m_mass);
//Added for auto fly height. Kitto Flora
//d.Vector3 pos = d.BodyGetPosition(Body);
float target_altitude = _parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y) + MinimumGroundFlightOffset;
if (_position.Z < target_altitude)
{
vec.Z += (target_altitude - _position.Z) * PID_P * 5.0f;
}
// end add Kitto Flora
}
if (PhysicsVector.isFinite(vec))
{
doForce(vec);
}
else
{
m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()");
m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
defects.Add(this);
// _parent_scene.RemoveCharacter(this);
// destroy avatar capsule and related ODE data
if (Amotor != IntPtr.Zero)
{
// Kill the Amotor
d.JointDestroy(Amotor);
Amotor = IntPtr.Zero;
}
//kill the Geometry
_parent_scene.waitForSpaceUnlock(_parent_scene.space);
if (Body != IntPtr.Zero)
{
//kill the body
d.BodyDestroy(Body);
Body = IntPtr.Zero;
}
if (Shell != IntPtr.Zero)
{
d.GeomDestroy(Shell);
_parent_scene.geom_name_map.Remove(Shell);
Shell = IntPtr.Zero;
}
}
}
public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, PhysicsVector size, float lod, bool isPhysical)
{
return(null);
}
/// <summary>
/// Adds a physical representation of the avatar to the Physics plugin
/// </summary>
public void AddToPhysicalScene(bool isFlying)
{
PhysicsScene scene = m_scene.PhysicsScene;
PhysicsVector pVec =
new PhysicsVector(AbsolutePosition.X, AbsolutePosition.Y,
AbsolutePosition.Z);
// Old bug where the height was in centimeters instead of meters
if (m_avHeight == 127.0f)
{
m_physicsActor = scene.AddAvatar(Firstname + "." + Lastname, pVec, new PhysicsVector(0, 0, 1.56f),
isFlying);
}
else
{
m_physicsActor = scene.AddAvatar(Firstname + "." + Lastname, pVec,
new PhysicsVector(0, 0, m_avHeight), isFlying);
}
scene.AddPhysicsActorTaint(m_physicsActor);
//m_physicsActor.OnRequestTerseUpdate += SendTerseUpdateToAllClients;
m_physicsActor.OnCollisionUpdate += PhysicsCollisionUpdate;
m_physicsActor.SubscribeEvents(1000);
m_physicsActor.LocalID = LocalId;
}
public virtual void RaiseOutOfBounds(PhysicsVector pos)
{
// Make a temporary copy of the event to avoid possibility of
// a race condition if the last subscriber unsubscribes
// immediately after the null check and before the event is raised.
OutOfBounds handler = OnOutOfBounds;
if (handler != null)
{
handler(pos);
}
}
public static PhysicsVector cross(PhysicsVector a, PhysicsVector b)
{
return(new PhysicsVector(a.Y * b.Z - a.Z * b.Y, a.Z * b.X - a.X * b.Z, a.X * b.Y - a.Y * b.X));
}
public abstract void AddAngularForce(PhysicsVector force, bool pushforce);
public BulletDotNETPrim(String primName, BulletDotNETScene parent_scene, PhysicsVector pos, PhysicsVector 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 = new PhysicsVector(0, 0, 0);
_velocity = new PhysicsVector();
_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 = new PhysicsVector();
m_rotationalVelocity = PhysicsVector.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 abstract void SetMomentum(PhysicsVector momentum);
public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, PhysicsVector size, float lod)
{
return(CreateMesh(primName, primShape, size, lod, false));
}
public void UpdatePositionAndVelocity()
{
if (!m_isSelected)
{
if (_parent == null)
{
PhysicsVector pv = new PhysicsVector(0, 0, 0);
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.setValues(tempAngularVelocity1.getX(), tempAngularVelocity1.getX(),
tempAngularVelocity1.getZ());
PhysicsVector l_position = new PhysicsVector();
Quaternion l_orientation = new Quaternion();
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 PhysicsVector(_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.IsIdentical(pv, 0.5f))
{
m_rotationalVelocity = pv;
}
else
{
m_rotationalVelocity.setValues(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;
}
}
}
public abstract void LockAngularMotion(PhysicsVector axis);
internal void EnableAxisMotor(PhysicsVector axislock)
{
if (m_aMotor != null)
DisableAxisMotor();
if (Body == null)
return;
if (Body.Handle == IntPtr.Zero)
return;
if (AxisLockAngleHigh != null && AxisLockAngleHigh.Handle != IntPtr.Zero)
AxisLockAngleHigh.Dispose();
m_aMotor = new btGeneric6DofConstraint(Body, _parent_scene.TerrainBody, _parent_scene.TransZero,
_parent_scene.TransZero, false);
float endNoLock = (360 * Utils.DEG_TO_RAD);
AxisLockAngleHigh = new btVector3((axislock.X == 0) ? 0 : endNoLock, (axislock.Y == 0) ? 0 : endNoLock, (axislock.Z == 0) ? 0 : endNoLock);
m_aMotor.setAngularLowerLimit(_parent_scene.VectorZero);
m_aMotor.setAngularUpperLimit(AxisLockAngleHigh);
m_aMotor.setLinearLowerLimit(AxisLockLinearLow);
m_aMotor.setLinearUpperLimit(AxisLockLinearHigh);
_parent_scene.getBulletWorld().addConstraint((btTypedConstraint)m_aMotor);
//m_aMotor.
}
public abstract void VehicleVectorParam(int param, PhysicsVector value);
public override void LockAngularMotion(PhysicsVector axis)
{
m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
m_taintAngularLock = new PhysicsVector(axis.X, axis.Y, axis.Z);
}
public abstract void SetMomentum(PhysicsVector momentum);
public override void AddForce(PhysicsVector force, bool pushforce)
{
m_forcelist.Add(force);
m_taintforce = true;
//m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
}
public override void LockAngularMotion(PhysicsVector axis)
{
}
public override void AddAngularForce(PhysicsVector force, bool pushforce)
{
m_angularforcelist.Add(force);
m_taintaddangularforce = true;
}
public override void AddAngularForce(PhysicsVector force, bool pushforce)
{
}
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;
}
public POSCharacter()
{
_velocity = new PhysicsVector();
_position = new PhysicsVector();
_acceleration = new PhysicsVector();
}
public void ProcessTaints(float timestep)
{
if (m_tainted_isPhysical != m_isPhysical)
{
if (m_tainted_isPhysical)
{
// Create avatar capsule and related ODE data
if (!(Shell == IntPtr.Zero && Body == IntPtr.Zero && Amotor == IntPtr.Zero))
{
m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - "
+ (Shell!=IntPtr.Zero ? "Shell ":"")
+ (Body!=IntPtr.Zero ? "Body ":"")
+ (Amotor!=IntPtr.Zero ? "Amotor ":""));
}
AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z, m_tensor);
_parent_scene.geom_name_map[Shell] = m_name;
_parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
_parent_scene.AddCharacter(this);
}
else
{
_parent_scene.RemoveCharacter(this);
// destroy avatar capsule and related ODE data
if (Amotor != IntPtr.Zero)
{
// Kill the Amotor
d.JointDestroy(Amotor);
Amotor = IntPtr.Zero;
}
//kill the Geometry
_parent_scene.waitForSpaceUnlock(_parent_scene.space);
if (Body != IntPtr.Zero)
{
//kill the body
d.BodyDestroy(Body);
Body = IntPtr.Zero;
}
if (Shell != IntPtr.Zero)
{
d.GeomDestroy(Shell);
_parent_scene.geom_name_map.Remove(Shell);
Shell = IntPtr.Zero;
}
}
m_isPhysical = m_tainted_isPhysical;
}
if (m_tainted_CAPSULE_LENGTH != CAPSULE_LENGTH)
{
if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero)
{
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());
d.BodyDestroy(Body);
d.GeomDestroy(Shell);
AvatarGeomAndBodyCreation(_position.X, _position.Y,
_position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2), m_tensor);
Velocity = new PhysicsVector(0f, 0f, 0f);
_parent_scene.geom_name_map[Shell] = m_name;
_parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
}
else
{
m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - "
+ (Shell==IntPtr.Zero ? "Shell ":"")
+ (Body==IntPtr.Zero ? "Body ":"")
+ (Amotor==IntPtr.Zero ? "Amotor ":""));
}
}
if (!m_taintPosition.IsIdentical(_position, 0.05f))
{
if (Body != IntPtr.Zero)
{
d.BodySetPosition(Body, m_taintPosition.X, m_taintPosition.Y, m_taintPosition.Z);
_position.X = m_taintPosition.X;
_position.Y = m_taintPosition.Y;
_position.Z = m_taintPosition.Z;
}
}
}
