public void Pitch(float angle)
{
Quaternion q = Quaternion.CreateFromAxisAngle(yAxis, angle);
Matrix4 m = Matrix4.CreateFromQuaternion(q);
Rotate(m);
if (!xAxis.IsFinite() || !zAxis.IsFinite())
{
throw new Exception("Non-finite in CoordinateFrame.Pitch()");
}
}
public override void AddForce(bool inTaintTime, OMV.Vector3 force)
{
if (force.IsFinite())
{
OMV.Vector3 addForce = Util.ClampV(force, BSParam.MaxAddForceMagnitude);
// DetailLog("{0},BSCharacter.addForce,call,force={1},push={2},inTaint={3}", LocalID, addForce, pushforce, inTaintTime);
PhysScene.TaintedObject(inTaintTime, LocalID, "BSCharacter.AddForce", delegate()
{
// Bullet adds this central force to the total force for this tick
// DetailLog("{0},BSCharacter.addForce,taint,force={1}", LocalID, addForce);
if (PhysBody.HasPhysicalBody)
{
// Bullet adds this central force to the total force for this tick.
// Deep down in Bullet:
// linearVelocity += totalForce / mass * timeStep;
PhysScene.PE.ApplyCentralForce(PhysBody, addForce);
PhysScene.PE.Activate(PhysBody, true);
}
if (m_moveActor != null)
{
m_moveActor.SuppressStationayCheckUntilLowVelocity();
}
});
}
else
{
m_log.WarnFormat("{0}: Got a NaN force applied to a character. LocalID={1}", LogHeader, LocalID);
return;
}
}
// Internal version that, if initializing, doesn't do all the updating of the physics engine
public void SetAvatarSize(OMV.Vector3 size, float feetOffset, bool initializing)
{
OMV.Vector3 newSize = size;
if (newSize.IsFinite())
{
// Old versions of ScenePresence passed only the height. If width and/or depth are zero,
// replace with the default values.
if (newSize.X == 0f)
{
newSize.X = BSParam.AvatarCapsuleDepth;
}
if (newSize.Y == 0f)
{
newSize.Y = BSParam.AvatarCapsuleWidth;
}
if (newSize.X < 0.01f)
{
newSize.X = 0.01f;
}
if (newSize.Y < 0.01f)
{
newSize.Y = 0.01f;
}
if (newSize.Z < 0.01f)
{
newSize.Z = BSParam.AvatarCapsuleHeight;
}
}
else
{
newSize = new OMV.Vector3(BSParam.AvatarCapsuleDepth, BSParam.AvatarCapsuleWidth, BSParam.AvatarCapsuleHeight);
}
// This is how much the avatar size is changing. Positive means getting bigger.
// The avatar altitude must be adjusted for this change.
float heightChange = newSize.Z - Size.Z;
_size = newSize;
Scale = ComputeAvatarScale(Size);
ComputeAvatarVolumeAndMass();
DetailLog("{0},BSCharacter.setSize,call,size={1},scale={2},density={3},volume={4},mass={5}",
LocalID, _size, Scale, Density, _avatarVolume, RawMass);
PhysScene.TaintedObject(LocalID, "BSCharacter.setSize", delegate()
{
if (PhysBody.HasPhysicalBody && PhysShape.physShapeInfo.HasPhysicalShape)
{
PhysScene.PE.SetLocalScaling(PhysShape.physShapeInfo, Scale);
UpdatePhysicalMassProperties(RawMass, true);
// Adjust the avatar's position to account for the increase/decrease in size
ForcePosition = new OMV.Vector3(RawPosition.X, RawPosition.Y, RawPosition.Z + heightChange / 2f);
// Make sure this change appears as a property update event
PhysScene.PE.PushUpdate(PhysBody);
}
});
}
private void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime)
{
if (force.IsFinite())
{
float magnitude = force.Length();
if (magnitude > BSParam.MaxAddForceMagnitude)
{
// Force has a limit
force = force / magnitude * BSParam.MaxAddForceMagnitude;
}
OMV.Vector3 addForce = force;
// DetailLog("{0},BSCharacter.addForce,call,force={1}", LocalID, addForce);
PhysicsScene.TaintedObject(inTaintTime, "BSCharacter.AddForce", delegate()
{
// Bullet adds this central force to the total force for this tick
// DetailLog("{0},BSCharacter.addForce,taint,force={1}", LocalID, addForce);
if (PhysBody.HasPhysicalBody)
{
PhysicsScene.PE.ApplyCentralForce(PhysBody, addForce);
}
});
}
else
{
m_log.WarnFormat("{0}: Got a NaN force applied to a character. LocalID={1}", LogHeader, LocalID);
return;
}
}
public override void AddForce(OMV.Vector3 force, bool pushforce)
{
if (force.IsFinite())
{
_force.X += force.X;
_force.Y += force.Y;
_force.Z += force.Z;
// m_log.DebugFormat("{0}: AddForce. adding={1}, newForce={2}", LogHeader, force, _force);
PhysicsScene.TaintedObject("BSCharacter.AddForce", delegate()
{
DetailLog("{0},BSCharacter.setAddForce,taint,addedForce={1}", LocalID, _force);
BulletSimAPI.SetObjectForce2(BSBody.ptr, _force);
});
}
else
{
m_log.ErrorFormat("{0}: Got a NaN force applied to a Character", LogHeader);
}
//m_lastUpdateSent = false;
}
private void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime)
{
if (force.IsFinite())
{
OMV.Vector3 addForce = Util.ClampV(force, BSParam.MaxAddForceMagnitude);
PhysicsScene.TaintedObject(inTaintTime, "BSCharacter.AddForce", delegate()
{
// Bullet adds this central force to the total force for this tick
if (PhysBody.HasPhysicalBody)
{
PhysicsScene.PE.ApplyCentralForce(PhysBody, addForce);
}
});
}
else
{
MainConsole.Instance.WarnFormat("{0}: Got a NaN force applied to a character. LocalID={1}", LogHeader, LocalID);
return;
}
}
public override void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime)
{
if (force.IsFinite())
{
OMV.Vector3 addForce = Util.ClampV(force, BSParam.MaxAddForceMagnitude);
// DetailLog("{0},BSCharacter.addForce,call,force={1}", LocalID, addForce);
PhysScene.TaintedObject(inTaintTime, LocalID, "BSCharacter.AddForce", delegate()
{
// Bullet adds this central force to the total force for this tick
// DetailLog("{0},BSCharacter.addForce,taint,force={1}", LocalID, addForce);
if (PhysBody.HasPhysicalBody)
{
PhysScene.PE.ApplyCentralForce(PhysBody, addForce);
}
});
}
else
{
m_log.WarnFormat("{0}: Got a NaN force applied to a character. LocalID={1}", LogHeader, LocalID);
return;
}
}
// Called just before the simulation step. Update the vertical position for hoverness.
void Mover(float timeStep)
{
// Don't do movement while the object is selected.
if (!isActive)
{
return;
}
// TODO: Decide if the step parameters should be changed depending on the avatar's
// state (flying, colliding, ...). There is code in ODE to do this.
// COMMENTARY: when the user is making the avatar walk, except for falling, the velocity
// specified for the avatar is the one that should be used. For falling, if the avatar
// is not flying and is not colliding then it is presumed to be falling and the Z
// component is not fooled with (thus allowing gravity to do its thing).
// When the avatar is standing, though, the user has specified a velocity of zero and
// the avatar should be standing. But if the avatar is pushed by something in the world
// (raising elevator platform, moving vehicle, ...) the avatar should be allowed to
// move. Thus, the velocity cannot be forced to zero. The problem is that small velocity
// errors can creap in and the avatar will slowly float off in some direction.
// So, the problem is that, when an avatar is standing, we cannot tell creaping error
// from real pushing.
// The code below uses whether the collider is static or moving to decide whether to zero motion.
m_velocityMotor.Step(timeStep);
m_controllingPrim.IsStationary = false;
// If we're not supposed to be moving, make sure things are zero.
if (m_velocityMotor.ErrorIsZero() && m_velocityMotor.TargetValue == OMV.Vector3.Zero)
{
// The avatar shouldn't be moving
m_velocityMotor.Zero();
if (m_controllingPrim.IsColliding)
{
// If we are colliding with a stationary object, presume we're standing and don't move around
if (!m_controllingPrim.ColliderIsMoving && !m_controllingPrim.VolumeDetect)
// new //if (!m_controllingPrim.ColliderIsMoving && !m_controllingPrim.ColliderIsVolumeDetect)
{
m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,collidingWithStationary,zeroingMotion",
m_controllingPrim.LocalID);
m_controllingPrim.IsStationary = true;
m_controllingPrim.ZeroMotion(true /* inTaintTime */);
}
// Standing has more friction on the ground
if (m_controllingPrim.Friction != BSParam.AvatarStandingFriction)
{
m_controllingPrim.Friction = BSParam.AvatarStandingFriction;
m_physicsScene.PE.SetFriction(m_controllingPrim.PhysBody, m_controllingPrim.Friction);
}
}
else
{
if (m_controllingPrim.Flying)
{
// Flying and not colliding and velocity nearly zero.
m_controllingPrim.ZeroMotion(true /* inTaintTime */);
}
else
{
//We are falling but are not touching any keys make sure not falling too fast
if (m_controllingPrim.RawVelocity.Z < BSParam.AvatarTerminalVelocity)
{
OMV.Vector3 slowingForce = new OMV.Vector3(0f, 0f, BSParam.AvatarTerminalVelocity - m_controllingPrim.RawVelocity.Z) * m_controllingPrim.Mass;
m_physicsScene.PE.ApplyCentralImpulse(m_controllingPrim.PhysBody, slowingForce);
}
}
}
m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,taint,stopping,target={1},colliding={2}",
m_controllingPrim.LocalID, m_velocityMotor.TargetValue, m_controllingPrim.IsColliding);
}
else
{
// Supposed to be moving.
OMV.Vector3 stepVelocity = m_velocityMotor.CurrentValue;
if (m_controllingPrim.Friction != BSParam.AvatarFriction)
{
// Probably starting up walking. Set friction to moving friction.
m_controllingPrim.Friction = BSParam.AvatarFriction;
m_physicsScene.PE.SetFriction(m_controllingPrim.PhysBody, m_controllingPrim.Friction);
}
// If falling, we keep the world's downward vector no matter what the other axis specify.
// The check for RawVelocity.Z < 0 makes jumping work (temporary upward force).
if (!m_controllingPrim.Flying && !m_controllingPrim.IsColliding)
{
if (m_controllingPrim.RawVelocity.Z < 0)
{
stepVelocity.Z = m_controllingPrim.RawVelocity.Z;
}
}
// Colliding and not flying with an upward force. The avatar must be trying to jump.
if (!m_controllingPrim.Flying && m_controllingPrim.IsColliding && stepVelocity.Z > 0)
{
// We allow the upward force to happen for this many frames.
m_jumpFrames = BSParam.AvatarJumpFrames;
m_jumpVelocity = stepVelocity.Z;
}
// The case where the avatar is not colliding and is not flying is special.
// The avatar is either falling or jumping and the user can be applying force to the avatar
// (force in some direction or force up or down).
// If the avatar has negative Z velocity and is not colliding, presume we're falling and keep the velocity.
// If the user is trying to apply upward force but we're not colliding, assume the avatar
// is trying to jump and don't apply the upward force if not touching the ground any more.
if (!m_controllingPrim.Flying && !m_controllingPrim.IsColliding)
{
// If upward velocity is being applied, this must be a jump and only allow that to go on so long
if (m_jumpFrames > 0)
{
// Since not touching the ground, only apply upward force for so long.
m_jumpFrames--;
stepVelocity.Z = m_jumpVelocity;
}
else
{
// Since we're not affected by anything, the avatar must be falling and we do not want that to be too fast.
if (m_controllingPrim.RawVelocity.Z < BSParam.AvatarTerminalVelocity)
{
stepVelocity.Z = BSParam.AvatarTerminalVelocity;
}
else
{
stepVelocity.Z = m_controllingPrim.RawVelocity.Z;
}
}
// DetailLog("{0},BSCharacter.MoveMotor,taint,overrideStepZWithWorldZ,stepVel={1}", LocalID, stepVelocity);
}
//Alicia: Maintain minimum height when flying.
// SL has a flying effect that keeps the avatar flying above the ground by some margin
if (m_controllingPrim.Flying)
{
float hover_height = m_physicsScene.TerrainManager.GetTerrainHeightAtXYZ(m_controllingPrim.RawPosition)
+ BSParam.AvatarFlyingGroundMargin;
if (m_controllingPrim.Position.Z < hover_height)
{
stepVelocity.Z += BSParam.AvatarFlyingGroundUpForce;
}
}
// 'stepVelocity' is now the speed we'd like the avatar to move in. Turn that into an instantanous force.
OMV.Vector3 moveForce = (stepVelocity - m_controllingPrim.RawVelocity) * m_controllingPrim.Mass;
// Should we check for move force being small and forcing velocity to zero?
// Add special movement force to allow avatars to walk up stepped surfaces.
moveForce += WalkUpStairs();
m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,move,stepVel={1},vel={2},mass={3},moveForce={4}",
m_controllingPrim.LocalID, stepVelocity, m_controllingPrim.RawVelocity, m_controllingPrim.Mass,
moveForce);
// i think this fix is working .. :-)
if (moveForce.IsFinite())
{
m_physicsScene.PE.ApplyCentralImpulse(m_controllingPrim.PhysBody, moveForce);
}
}
// a bit of jump processing...
if (m_preJumpStart != 0 && Util.EnvironmentTickCountSubtract(m_preJumpStart) > 300)
{
m_controllingPrim.IsPreJumping = false;
m_controllingPrim.IsJumping = true;
m_preJumpStart = 0;
OMV.Vector3 target = m_jumpDirection;
target.X *= 1.5f;
target.Y *= 1.5f;
target.Z *= 2.5f; //Scale so that we actually jump
SetVelocityAndTargetInternal(m_controllingPrim.RawVelocity, target, false, 1);
m_jumpStart = Util.EnvironmentTickCount();
}
if (!m_jumpFallState && m_jumpStart != 0 && Util.EnvironmentTickCountSubtract(m_jumpStart) > 500)
{
OMV.Vector3 newTarget = m_controllingPrim.RawVelocity;
newTarget.X *= 1.5f; //Scale so that the jump looks correct
newTarget.Y *= 1.5f;
newTarget.Z *= -0.2f;
SetVelocityAndTargetInternal(m_controllingPrim.RawVelocity, newTarget, false, 3);
m_jumpFallState = true;
}
else if (m_jumpFallState && m_jumpStart != 0 && m_controllingPrim.IsColliding &&
Util.EnvironmentTickCountSubtract(m_jumpStart) > 1500 ||
(m_jumpStart != 0 && Util.EnvironmentTickCountSubtract(m_jumpStart) > 10000)) //Fallback
{
//Reset everything in case something went wrong
m_disallowTargetVelocitySet = false;
m_jumpFallState = false;
m_controllingPrim.IsPreJumping = false;
m_controllingPrim.IsJumping = false;
m_jumpStart = 0;
m_preJumpStart = 0;
}
}
// Called just before the simulation step. Update the vertical position for hoverness.
private void Mover(float timeStep)
{
// Don't do movement while the object is selected.
if (!isActive)
{
return;
}
// TODO: Decide if the step parameters should be changed depending on the avatar's
// state (flying, colliding, ...). There is code in ODE to do this.
// COMMENTARY: when the user is making the avatar walk, except for falling, the velocity
// specified for the avatar is the one that should be used. For falling, if the avatar
// is not flying and is not colliding then it is presumed to be falling and the Z
// component is not fooled with (thus allowing gravity to do its thing).
// When the avatar is standing, though, the user has specified a velocity of zero and
// the avatar should be standing. But if the avatar is pushed by something in the world
// (raising elevator platform, moving vehicle, ...) the avatar should be allowed to
// move. Thus, the velocity cannot be forced to zero. The problem is that small velocity
// errors can creap in and the avatar will slowly float off in some direction.
// So, the problem is that, when an avatar is standing, we cannot tell creaping error
// from real pushing.
// The code below uses whether the collider is static or moving to decide whether to zero motion.
m_velocityMotor.Step(timeStep);
m_controllingPrim.IsStationary = false;
// If we're not supposed to be moving, make sure things are zero.
if (m_velocityMotor.ErrorIsZero() && m_velocityMotor.TargetValue == OMV.Vector3.Zero)
{
// The avatar shouldn't be moving
m_velocityMotor.Zero();
if (m_controllingPrim.IsColliding)
{
// If we are colliding with a stationary object, presume we're standing and don't move around
if (!m_controllingPrim.ColliderIsMoving)
{
m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,collidingWithStationary,zeroingMotion", m_controllingPrim.LocalID);
m_controllingPrim.IsStationary = true;
m_controllingPrim.ZeroMotion(true /* inTaintTime */);
}
// Standing has more friction on the ground
if (m_controllingPrim.Friction != BSParam.AvatarStandingFriction)
{
m_controllingPrim.Friction = BSParam.AvatarStandingFriction;
m_physicsScene.PE.SetFriction(m_controllingPrim.PhysBody, m_controllingPrim.Friction);
}
}
else
{
if (m_controllingPrim.Flying)
{
// Flying and not collising and velocity nearly zero.
m_controllingPrim.ZeroMotion(true /* inTaintTime */);
}
}
m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,taint,stopping,target={1},colliding={2}",
m_controllingPrim.LocalID, m_velocityMotor.TargetValue, m_controllingPrim.IsColliding);
}
else
{
// Supposed to be moving.
OMV.Vector3 stepVelocity = m_velocityMotor.CurrentValue;
if (m_controllingPrim.Friction != BSParam.AvatarFriction)
{
// Probably starting up walking. Set friction to moving friction.
m_controllingPrim.Friction = BSParam.AvatarFriction;
m_physicsScene.PE.SetFriction(m_controllingPrim.PhysBody, m_controllingPrim.Friction);
}
// If falling, we keep the world's downward vector no matter what the other axis specify.
// The check for RawVelocity.Z < 0 makes jumping work (temporary upward force).
if (!m_controllingPrim.Flying && !m_controllingPrim.IsColliding)
{
if (m_controllingPrim.RawVelocity.Z < 0)
{
stepVelocity.Z = m_controllingPrim.RawVelocity.Z;
}
// DetailLog("{0},BSCharacter.MoveMotor,taint,overrideStepZWithWorldZ,stepVel={1}", LocalID, stepVelocity);
}
// 'stepVelocity' is now the speed we'd like the avatar to move in. Turn that into an instantanous force.
OMV.Vector3 moveForce = (stepVelocity - m_controllingPrim.RawVelocity) * m_controllingPrim.Mass;
// Should we check for move force being small and forcing velocity to zero?
// Add special movement force to allow avatars to walk up stepped surfaces.
moveForce += WalkUpStairs();
m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,move,stepVel={1},vel={2},mass={3},moveForce={4}",
m_controllingPrim.LocalID, stepVelocity, m_controllingPrim.RawVelocity, m_controllingPrim.Mass, moveForce);
if (moveForce.IsFinite())
{
m_physicsScene.PE.ApplyCentralImpulse(m_controllingPrim.PhysBody, moveForce);
}
}
if (m_preJumpStart != 0 && Util.EnvironmentTickCountSubtract(m_preJumpStart) > 300)
{
m_controllingPrim.IsPreJumping = false;
m_controllingPrim.IsJumping = true;
m_preJumpStart = 0;
OMV.Vector3 target = m_jumpDirection;
target.X *= 1.5f;
target.Y *= 1.5f;
target.Z *= 2.5f;//Scale so that we actually jump
SetVelocityAndTargetInternal(m_controllingPrim.RawVelocity, target, false, 1);
m_jumpStart = Util.EnvironmentTickCount();
}
if (!m_jumpFallState && m_jumpStart != 0 && Util.EnvironmentTickCountSubtract(m_jumpStart) > 500)
{
OMV.Vector3 newTarget = m_controllingPrim.RawVelocity;
newTarget.X *= 1.5f;//Scale so that the jump looks correct
newTarget.Y *= 1.5f;
newTarget.Z *= -0.2f;
SetVelocityAndTargetInternal(m_controllingPrim.RawVelocity, newTarget, false, 3);
m_jumpFallState = true;
}
else if (m_jumpFallState && m_jumpStart != 0 && m_controllingPrim.IsColliding && Util.EnvironmentTickCountSubtract(m_jumpStart) > 1500 ||
(m_jumpStart != 0 && Util.EnvironmentTickCountSubtract(m_jumpStart) > 10000))//Fallback
{
//Reset everything in case something went wrong
m_disallowTargetVelocitySet = false;
m_jumpFallState = false;
m_controllingPrim.IsPreJumping = false;
m_controllingPrim.IsJumping = false;
m_jumpStart = 0;
m_preJumpStart = 0;
}
}
