/// <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());
vel *= 0.25f;
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) * 10;
}
}
if (m_flying)
{
// Slight PID correction
vec.Z += (((-1 * m_parent_scene.gravityz) * m_mass) * 1.5f);
//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);
}
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 posChange = new Vector3();
posChange.X = newpos.getX() - m_lastPositionVector.getX();
posChange.Y = newpos.getY() - m_lastPositionVector.getY();
posChange.Z = newpos.getZ() - m_lastPositionVector.getZ();
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;
}
/// <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();
}
/// <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!
PhysicsVector vec = new PhysicsVector(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 PhysicsVector(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();
}