/// <summary>
/// Applies the impulse.
/// </summary>
/// <param name="impulse">The dir.</param>
/// <param name="pos">The pos.</param>
public override void ApplyImpulse(Vector3 impulse, Vector3D pos)
{
impulse.AssertIsValid();
pos.AssertIsValid();
System.Diagnostics.Debug.Assert(IsInWorld == true);
var offset = MyPhysics.Clusters.GetObjectOffset(ClusterObjectID);
var posF = (Vector3)(pos - offset);
RigidBody.ApplyPointImpulse(impulse, posF);
}
/// <summary>
/// Applies external force to the physics object.
/// </summary>
/// <param name="type">The type.</param>
/// <param name="force">The force.</param>
/// <param name="position">The position.</param>
/// <param name="torque">The torque.</param>
public override void AddForce(MyPhysicsForceType type, Vector3? force, Vector3D? position, Vector3? torque)
{
force.AssertIsValid();
position.AssertIsValid();
torque.AssertIsValid();
System.Diagnostics.Debug.Assert(IsInWorld == true);
if (IsStatic)
return;
switch (type)
{
case MyPhysicsForceType.ADD_BODY_FORCE_AND_BODY_TORQUE:
{
if (RigidBody != null)
{
Matrix tempM = RigidBody.GetRigidBodyMatrix();
tempM.Translation = Vector3.Zero;
if (force != null && !MyUtils.IsZero(force.Value))
{
Vector3 tmpForce = Vector3.Transform(force.Value, tempM);
//RigidBody.Activate(true);
//RigidBody.ApplyForce(MyEngineConstants.PHYSICS_STEP_SIZE_IN_SECONDS, tmpForce * 0.0001f);
RigidBody.ApplyLinearImpulse(tmpForce * MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS * MyFakes.SIMULATION_SPEED);
//RigidBody.ApplyCentralImpulse(tmpForce);
}
if (torque != null && !MyUtils.IsZero(torque.Value))
{
Vector3 tmpTorque = Vector3.Transform(torque.Value, tempM);
//SharpDX.Vector3 tmpTorque = SharpDXHelper.ToSharpDX(torque.Value);
// RigidBody.Activate(true);
//RigidBody.UpdateInertiaTensor();
//RigidBody.ApplyTorque(MyEngineConstants.PHYSICS_STEP_SIZE_IN_SECONDS, tmpTorque * 0.0001f);
RigidBody.ApplyAngularImpulse(tmpTorque * MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS * MyFakes.SIMULATION_SPEED);
//RigidBody.ApplyTorqueImpulse(tmpTorque);
}
}
if (CharacterProxy != null)
{
Matrix tempM = Entity.WorldMatrix;
tempM.Translation = Vector3.Zero;
if (force != null && !MyUtils.IsZero(force.Value))
{
Vector3 tmpForce = Vector3.Transform(force.Value, tempM);
CharacterProxy.ApplyLinearImpulse(tmpForce * MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS * MyFakes.SIMULATION_SPEED);
}
if (torque != null && !MyUtils.IsZero(torque.Value))
{
Vector3 tmpTorque = Vector3.Transform(torque.Value, tempM);
CharacterProxy.ApplyAngularImpulse(tmpTorque * MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS * MyFakes.SIMULATION_SPEED);
}
}
if (Ragdoll != null && Ragdoll.IsAddedToWorld && !Ragdoll.IsKeyframed)
{
foreach (var rigidBody in Ragdoll.RigidBodies)
{
if (rigidBody != null)
{
Matrix tempM = rigidBody.GetRigidBodyMatrix();
tempM.Translation = Vector3.Zero;
if (force != null && !MyUtils.IsZero(force.Value))
{
Vector3 tmpForce = Vector3.Transform(force.Value, tempM) * rigidBody.Mass / Ragdoll.Mass;
//RigidBody.Activate(true);
//RigidBody.ApplyForce(MyEngineConstants.PHYSICS_STEP_SIZE_IN_SECONDS, tmpForce * 0.0001f);
rigidBody.ApplyLinearImpulse(tmpForce * MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS * MyFakes.SIMULATION_SPEED);
//RigidBody.ApplyCentralImpulse(tmpForce);
}
if (torque != null && !MyUtils.IsZero(torque.Value))
{
Vector3 tmpTorque = Vector3.Transform(torque.Value, tempM) * rigidBody.Mass / Ragdoll.Mass;
//SharpDX.Vector3 tmpTorque = SharpDXHelper.ToSharpDX(torque.Value);
// RigidBody.Activate(true);
//RigidBody.UpdateInertiaTensor();
//RigidBody.ApplyTorque(MyEngineConstants.PHYSICS_STEP_SIZE_IN_SECONDS, tmpTorque * 0.0001f);
rigidBody.ApplyAngularImpulse(tmpTorque * MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS * MyFakes.SIMULATION_SPEED);
//RigidBody.ApplyTorqueImpulse(tmpTorque);
}
}
}
}
}
break;
case MyPhysicsForceType.APPLY_WORLD_IMPULSE_AND_WORLD_ANGULAR_IMPULSE:
{
if (RigidBody != null)
{
var offset = MyPhysics.Clusters.GetObjectOffset(ClusterObjectID);
if (force.HasValue && position.HasValue)
{
//this.RigidBody.ApplyImpulse(force.Value, position.Value);
//RigidBody.Activate(true);
RigidBody.ApplyPointImpulse(force.Value, (Vector3)(position.Value - offset));
}
if (torque.HasValue)
{
//RigidBody.Activate(true);
//RigidBody.ApplyTorque(MyEngineConstants.PHYSICS_STEP_SIZE_IN_SECONDS, torque.Value * 0.0001f);
RigidBody.ApplyAngularImpulse(torque.Value * MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS * MyFakes.SIMULATION_SPEED);
}
}
if (CharacterProxy != null && force.HasValue && position.HasValue)
{
CharacterProxy.ApplyLinearImpulse(force.Value);
}
if (Ragdoll != null && Ragdoll.IsAddedToWorld && !Ragdoll.IsKeyframed)
{
foreach (var rigidBody in Ragdoll.RigidBodies)
{
if (rigidBody != null)
{
var offset = MyPhysics.Clusters.GetObjectOffset(ClusterObjectID);
if (force.HasValue && position.HasValue)
{
//this.RigidBody.ApplyImpulse(force.Value, position.Value);
//RigidBody.Activate(true);
rigidBody.ApplyPointImpulse(force.Value * rigidBody.Mass / Ragdoll.Mass, (Vector3)(position.Value - offset));
}
if (torque.HasValue)
{
//RigidBody.Activate(true);
//RigidBody.ApplyTorque(MyEngineConstants.PHYSICS_STEP_SIZE_IN_SECONDS, torque.Value * 0.0001f);
rigidBody.ApplyAngularImpulse(torque.Value * MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS * MyFakes.SIMULATION_SPEED * rigidBody.Mass / Ragdoll.Mass);
}
}
}
}
}
break;
case MyPhysicsForceType.APPLY_WORLD_FORCE:
{
if (RigidBody != null)
{
var offset = MyPhysics.Clusters.GetObjectOffset(ClusterObjectID);
if (force != null && !MyUtils.IsZero(force.Value))
{
if (position.HasValue)
{
Vector3 point = position.Value - offset;
RigidBody.ApplyForce(MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS, force.Value, point);
}
else
RigidBody.ApplyForce(MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS, force.Value);
}
}
if (Ragdoll != null && Ragdoll.IsAddedToWorld && !Ragdoll.IsKeyframed)
{
foreach (var rigidBody in Ragdoll.RigidBodies)
{
if (rigidBody != null)
{
var offset = MyPhysics.Clusters.GetObjectOffset(ClusterObjectID);
if (force != null && !MyUtils.IsZero(force.Value))
{
if (position.HasValue)
{
Vector3 point = position.Value - offset;
rigidBody.ApplyForce(MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS, force.Value * rigidBody.Mass / Ragdoll.Mass, point);
}
else
rigidBody.ApplyForce(MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS, force.Value * rigidBody.Mass / Ragdoll.Mass);
}
}
}
}
}
break;
default:
{
Debug.Fail("Unhandled enum!");
}
break;
}
}
// Updates spectator position (spring connected to desired position)
public override void UpdateAfterSimulation()
{
Sandbox.Game.Entities.IMyControllableEntity controlledEntity = MySession.ControlledEntity as Sandbox.Game.Entities.IMyControllableEntity;
if (controlledEntity == null)
return;
var headMatrix = controlledEntity.GetHeadMatrix(true);
if (controlledEntity is MyCharacter)
{
var character = controlledEntity as MyCharacter;
headMatrix = character.Get3rdBoneMatrix(true, true);
}
m_targetOrientation = (Matrix)headMatrix.GetOrientation();
m_target = headMatrix.Translation;
//VRageRender.MyRenderProxy.DebugDrawAxis(headMatrix, 1, false);
UpdateCurrentSpring();
m_transformedLookAt = Vector3D.Transform(m_lookAt, m_targetOrientation);
m_desiredPosition = m_target + m_transformedLookAt;
//m_position = m_desiredPosition;
//m_velocity = Vector3.Zero;
// Calculate spring force
Vector3D stretch = m_position - m_desiredPosition;
Vector3D force = -m_currentSpring.Stiffness * stretch - m_currentSpring.Damping * m_velocity;
force.AssertIsValid();
// Apply acceleration
Vector3 acceleration = (Vector3)force / m_currentSpring.Mass;
m_velocity += acceleration * MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS;
m_velocity.AssertIsValid();
// Apply velocity
if (!Sandbox.Game.Multiplayer.Sync.IsServer)
{ //We are not able to interpolate camera correctly if position is updated through server
m_position = m_desiredPosition;
}
else
{
m_position += m_velocity * MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS;
}
m_position.AssertIsValid();
// Limit backward distance from target
double backward = Vector3D.Dot((Vector3D)m_targetOrientation.Backward, (m_target - m_position));
if (backward > -BACKWARD_CUTOFF)
{
m_position += (Vector3D)m_targetOrientation.Backward * (backward + BACKWARD_CUTOFF);
}
// Roll spring
Quaternion targetOrientation = Quaternion.CreateFromRotationMatrix(m_targetOrientation);
// Computes angle difference between current and target orientation
var angleDifference = (float)Math.Acos(MathHelper.Clamp(Quaternion.Dot(m_orientation, targetOrientation), -1, 1));
// Normalize angle
angleDifference = angleDifference > MathHelper.PiOver2 ? MathHelper.Pi - angleDifference : angleDifference;
// Compute spring physics
float angleForce = -AngleSpring.Stiffness * angleDifference - AngleSpring.Damping * m_angleVelocity;
m_angleVelocity += angleForce / AngleSpring.Mass * MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS;
if (angleDifference > 0)
{
float factor = Math.Abs(m_angleVelocity * MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS / angleDifference);
if (angleDifference > MathHelper.PiOver4)
{
factor = Math.Max(factor, 1.0f - MathHelper.PiOver4 / angleDifference);
}
factor = MathHelper.Clamp(factor, 0, 1);
m_orientation = Quaternion.Slerp(m_orientation, targetOrientation, factor);
m_orientationMatrix = Matrix.CreateFromQuaternion(m_orientation);
}
if (m_saveSettings)
{
MySession.SaveControlledEntityCameraSettings(false);
m_saveSettings = false;
}
++m_updateCount;
}
/// <summary>
/// Applies external force to the physics object.
/// </summary>
/// <param name="type">The type.</param>
/// <param name="force">The force.</param>
/// <param name="position">The position.</param>
/// <param name="torque">The torque.</param>
public override void AddForce(MyPhysicsForceType type, Vector3? force, Vector3D? position, Vector3? torque)
{
force.AssertIsValid();
position.AssertIsValid();
torque.AssertIsValid();
System.Diagnostics.Debug.Assert(IsInWorld == true || IsWelded);
if (IsStatic)
return;
Matrix transform;
switch (type)
{
case MyPhysicsForceType.ADD_BODY_FORCE_AND_BODY_TORQUE:
{
if (RigidBody != null)
{
transform = RigidBody.GetRigidBodyMatrix();
AddForceTorqueBody(force, torque, RigidBody, ref transform);
}
if (CharacterProxy != null)
{
transform = Entity.WorldMatrix;
AddForceTorqueBody(force, torque, CharacterProxy.GetHitRigidBody(), ref transform);
}
if (Ragdoll != null && Ragdoll.IsAddedToWorld && !Ragdoll.IsKeyframed)
{
transform = Entity.WorldMatrix;
ApplyForceTorqueOnRagdoll(force, torque, Ragdoll, ref transform);
}
}
break;
case MyPhysicsForceType.APPLY_WORLD_IMPULSE_AND_WORLD_ANGULAR_IMPULSE:
{
ApplyImplusesWorld(force, position, torque, RigidBody);
if (CharacterProxy != null && force.HasValue && position.HasValue)
{
CharacterProxy.ApplyLinearImpulse(force.Value);
}
if (Ragdoll != null && Ragdoll.IsAddedToWorld && !Ragdoll.IsKeyframed)
{
ApplyImpuseOnRagdoll(force, position, torque, Ragdoll);
}
}
break;
case MyPhysicsForceType.APPLY_WORLD_FORCE:
{
ApplyForceWorld(force, position, RigidBody);
if (Ragdoll != null && Ragdoll.IsAddedToWorld && !Ragdoll.IsKeyframed)
{
ApplyForceOnRagdoll(force, position, Ragdoll);
}
}
break;
default:
{
Debug.Fail("Unhandled enum!");
}
break;
}
}
private void ProcessSpringCalculation()
{
Vector3D stretch = m_position - m_desiredPosition;
Vector3D force = -m_currentSpring.Stiffness * stretch - m_currentSpring.Dampening * m_velocity;
force.AssertIsValid();
// Apply acceleration
Vector3 acceleration = (Vector3) force / m_currentSpring.Mass;
m_velocity += acceleration * MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS;
m_velocity.AssertIsValid();
// Apply velocity
m_position += m_velocity * MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS;
m_position.AssertIsValid();
}