protected override bool OnContact(ref MyRBSolverConstraint constraint)
{
MyEntity collidedEntity = constraint.GetOtherEntity(this);
/*
// we take the closest collided entity
if (m_collidedEntity == null || Vector3.Distance(this.GetPosition(), constraint.m_ContactPoint) < Vector3.Distance(this.GetPosition(), m_collisionPoint.Value))
{
m_collisionPoint = constraint.m_ContactPoint;
if (collidedEntity != m_collidedEntity)
{
m_collidedEntity = collidedEntity;
if (m_collidedEntity is MyVoxelMap && m_usedAmmo.AmmoType == MyMwcObjectBuilder_SmallShip_Ammo_TypesEnum.Cannon_Tunnel_Buster)
{
if (m_penetratedVoxelMap == null)
{
m_penetratedVoxelMap = m_collidedEntity as MyVoxelMap;
m_penetrationOrigin = GetPosition();
m_wasPenetration = true;
}
return false;
}
}
}
if (m_penetratedVoxelMap != null)
return false;
*/
return true;
}
/// <summary>
/// Solves the solver rbos and constraints through iterations given by the island. This is a newton based solver.
/// </summary>
private void Solve(float dt)
{
Vector3 v1 = new Vector3();
Vector3 v2 = new Vector3();
for (int iteration = 0; iteration < m_Island.IterationCount; iteration++)
{
for (int i = 0; i < m_SolverConstraints.Count; i++)
{
MyRBSolverConstraint sc = m_SolverConstraints[i];
MyRBSolverBody b1 = sc.m_SolverBody1;
MyRBSolverBody b2 = sc.m_SolverBody2;
// compute relative constraint velocity
if (b1.m_State == MyRBSolverBody.SolverBodyState.SBS_Static)
{
v1 = Vector3.Zero;
}
else
{
v1 = b1.m_LinearVelocity + Vector3.Cross(b1.m_AngularVelocity, sc.m_Body1LocalPoint);
}
if (b2.m_State == MyRBSolverBody.SolverBodyState.SBS_Static)
{
v2 = Vector3.Zero;
}
else
{
v2 = b2.m_LinearVelocity + Vector3.Cross(b2.m_AngularVelocity, sc.m_Body2LocalPoint);
}
float current = Vector3.Dot(v2, sc.m_Normal) - Vector3.Dot(v1, sc.m_Normal);
float diff = sc.m_Target - current;
if (sc.m_Magnitude < sc.m_MinMagnitude || sc.m_Magnitude > sc.m_MaxMagnitude ||
(diff < -m_CollisionEpsilon && sc.m_Magnitude > sc.m_MinMagnitude) ||
(diff > m_CollisionEpsilon && sc.m_Magnitude < sc.m_MaxMagnitude))
{
// Alter magnitude
float alter = (diff / sc.m_Affection) * 0.5f;
sc.m_Magnitude += alter;
//Ensure magnitude is in valid range
if (sc.m_Magnitude > sc.m_MaxMagnitude)
{
alter -= sc.m_Magnitude - sc.m_MaxMagnitude;
sc.m_Magnitude = sc.m_MaxMagnitude;
}
else if (sc.m_Magnitude < sc.m_MinMagnitude)
{
alter -= sc.m_Magnitude - sc.m_MinMagnitude;
sc.m_Magnitude = sc.m_MinMagnitude;
}
//Apply impulse
if (b1.m_State == MyRBSolverBody.SolverBodyState.SBS_Dynamic)
{
b1.m_LinearVelocity -= sc.m_Normal * (b1.m_OneOverMass * alter);
Vector3 tempV = b1.m_LinearVelocity;
tempV.Normalize();
if (b2.m_State == MyRBSolverBody.SolverBodyState.SBS_Dynamic)
{
float massRatio = 1.0f / (b1.m_OneOverMass / b2.m_OneOverMass);
//If mass of object1 is more than object2, do not apply restitution at all
float rest = MathHelper.Lerp(sc.m_Restitution, 1, massRatio - 0.5f);
sc.m_Restitution = MathHelper.Clamp(rest, sc.m_Restitution, 1);
}
b1.m_AngularVelocity -= sc.m_Body1LocalPointCrossedNormal * (alter);
MinerWars.AppCode.Game.Utils.MyUtils.AssertIsValid(b1.m_AngularVelocity);
}
if (b2.m_State == MyRBSolverBody.SolverBodyState.SBS_Dynamic)
{
b2.m_LinearVelocity += sc.m_Normal * (b2.m_OneOverMass * alter);
Vector3 tempV = b2.m_LinearVelocity;
tempV.Normalize();
if (b1.m_State == MyRBSolverBody.SolverBodyState.SBS_Dynamic)
{
float massRatio = 1.0f / (b2.m_OneOverMass / b1.m_OneOverMass);
//If mass of object2 is more than object1, do not apply restitution at all
float rest = MathHelper.Lerp(sc.m_Restitution, 1, massRatio - 0.5f);
sc.m_Restitution = MathHelper.Clamp(rest, sc.m_Restitution, 1);
}
b2.m_AngularVelocity += sc.m_Body2LocalPointCrossedNormal * (alter);
MinerWars.AppCode.Game.Utils.MyUtils.AssertIsValid(b2.m_AngularVelocity);
}
}
}
foreach (KeyValuePair <MyRigidBody, MyRBSolverBody> kvp in m_SolverBodies)
{
MyRBSolverBody body = kvp.Value;
float cdt = dt / (float)(iteration + 1);
if (body.m_State == MyRBSolverBody.SolverBodyState.SBS_Dynamic)
{
//Integrate velocities
body.m_LinearVelocity += body.m_LinearAcceleration * (cdt);
body.m_AngularVelocity += body.m_AngularAcceleration * (cdt);
MinerWars.AppCode.Game.Utils.MyUtils.AssertIsValid(body.m_AngularVelocity);
if (body.m_MaxAngularVelocity > 0.0f && body.m_AngularVelocity.Length() > body.m_MaxAngularVelocity)
{
body.m_AngularVelocity = MyMwcUtils.Normalize(body.m_AngularVelocity);
body.m_AngularVelocity *= body.m_MaxAngularVelocity;
}
if (body.m_MaxLinearVelocity > 0.0f && body.m_LinearVelocity.Length() > body.m_MaxLinearVelocity)
{
body.m_LinearVelocity = MyMwcUtils.Normalize(body.m_LinearVelocity);
body.m_LinearVelocity *= body.m_MaxLinearVelocity;
}
}
}
}
//Integrate position and orientation
foreach (KeyValuePair <MyRigidBody, MyRBSolverBody> kvp in m_SolverBodies)
{
MyRBSolverBody body = kvp.Value;
if (body.m_State != MyRBSolverBody.SolverBodyState.SBS_Static)
{
Vector3 transl = body.m_Matrix.Translation + body.m_LinearVelocity * (dt);
Vector3 dir = body.m_AngularVelocity;
float ang = dir.Length();
if (ang > 0.0f)
{
body.m_Matrix.Translation = Vector3.Zero;
Vector3.Divide(ref dir, ang, out dir); // dir /= ang;
ang *= dt;
Matrix rot;
Matrix.CreateFromAxisAngle(ref dir, ang, out rot);
Matrix.Multiply(ref body.m_Matrix, ref rot, out body.m_Matrix);
MyPhysicsUtils.Orthonormalise(ref body.m_Matrix);
}
body.m_Matrix.Translation = transl;
}
}
}
/// <summary>
/// Called when [contact] with entity.
/// </summary>
/// <param name="constraint">The constraint.</param>
/// <returns></returns>
protected override bool OnContact(ref MyRBSolverConstraint constraint)
{
var entityA = (constraint.m_SolverBody1.m_RigidBody.m_UserData as MyPhysicsBody).Entity;
var entityB = (constraint.m_SolverBody2.m_RigidBody.m_UserData as MyPhysicsBody).Entity;
Debug.Assert(entityA == this || entityB == this, "Contact is not with this smallship");
var otherEntity = entityA == this ? entityB : entityA;
if (!PlayerSimulation)
{
if (otherEntity is MyEntityDetector)
return true;
return false;
}
// For ammo base, don't use "lazy" collision reaction
if (otherEntity is MyAmmoBase)
return true;
var otherSmallShip = otherEntity as MySmallShip;
if (otherSmallShip != null)
{
//// disable collisions of player friends with player
//if (otherSmallShip.Leader == MySession.PlayerShip && Leader == MySession.PlayerShip)
//{
// return false;
//}
if (otherSmallShip.Leader == this && this == MySession.PlayerShip)
{
return false;
}
// disable collisions of player and other peers
if (MyMultiplayerPeers.Static.IsStarted)
{
bool a = false;
bool b = false;
foreach (var player in MyMultiplayerPeers.Static.Players)
{
a |= player.Ship == this;
b |= player.Ship == this;
if (a && b)
{
return false;
}
}
}
if (this == MySession.PlayerShip && MySession.PlayerFriends.Contains(otherSmallShip))
{
return false;
}
}
if ((otherEntity is MySmallShip || otherEntity is MyCargoBox) && this == MySession.PlayerShip)
{
//There is 0 because otherwise collision with marcus is too bouncy
this.Physics.AngularDamping = m_angularDampingSlowdown;
//return true;
}
//this.Physics.AngularDamping = 0.0f;
m_hasCollided = true;
MyUtils.AssertIsValid(Physics.LinearVelocity);
MyUtils.AssertIsValid(constraint.m_Normal);
if (MyMwcUtils.IsZero(Physics.LinearVelocity))// can be because ship switches between static during harvest
return true;
m_collisionFriction = Math.Abs(Vector3.Dot(Vector3.Normalize(Physics.LinearVelocity), constraint.m_Normal));
if (constraint.m_SolverBody1.m_RigidBody.m_UserData == this.Physics)
{
// Marek Rosa: I have disabled Ales' trick with rotating in one direction after contact, because it seems to be not needed after I switched small ship from sphere to a box
constraint.m_Body1LocalPointCrossedNormal = Vector3.Zero;
//constraint.m_Body1LocalPointCrossedNormal *= 0.02f;
}
else if (constraint.m_SolverBody2.m_RigidBody.m_UserData == this.Physics)
{
// Marek Rosa: I have disabled Ales' trick with rotating in one direction after contact, because it seems to be not needed after I switched small ship from sphere to a box
constraint.m_Body2LocalPointCrossedNormal = Vector3.Zero;
//constraint.m_Body2LocalPointCrossedNormal *= 0.02f;
}
return true;
}
/// <summary>
/// creates and prepare the solver constraint
/// </summary>
private void AddConstraint(MyRBContactConstraint rbc)
{
if (rbc.GetRBElementInteraction().GetRigidBody1().IsStatic() && rbc.GetRBElementInteraction().GetRigidBody2().IsStatic())
{
return;
}
for (int i = 0; i < rbc.m_NumCollPts; i++)
{
MyRBSolverConstraint rbsc = m_SolverConstaintPool.Allocate();
rbsc.Clear();
MyCollPointInfo pointInfo = rbc.m_PointInfo[i];
MyRBSolverBody body1 = null;
MyRBSolverBody body2 = null;
MyRigidBody rbo1 = rbc.GetRBElementInteraction().GetRigidBody1();
MyRigidBody rbo2 = rbc.GetRBElementInteraction().GetRigidBody2();
if (!m_SolverBodies.TryGetValue(rbo1, out body1))
{
body1 = AddRigidBody(rbo1);
}
if (!m_SolverBodies.TryGetValue(rbo2, out body2))
{
body2 = AddRigidBody(rbo2);
}
if ((rbo1.GetFlags() & RigidBodyFlag.RBF_DISABLE_COLLISION_RESPONCE) > 0)
{
continue;
}
if ((rbo2.GetFlags() & RigidBodyFlag.RBF_DISABLE_COLLISION_RESPONCE) > 0)
{
continue;
}
rbsc.m_SolverBody1 = body1;
rbsc.m_SolverBody2 = body2;
MyRBMaterial material1 = rbc.GetRBElementInteraction().RBElement1.RBElementMaterial;
MyRBMaterial material2 = rbc.GetRBElementInteraction().RBElement2.RBElementMaterial;
float restitution = material1.NominalRestitution * material2.NominalRestitution;
rbsc.m_RBConstraint = rbc;
rbsc.m_Magnitude = rbc.Magnitude * m_SolutionDamping;
//This is a contact constraint
Vector3 globalVel1 = new Vector3();
Vector3 globalVel2 = new Vector3();
rbc.GetRBElementInteraction().GetRigidBody1().GetGlobalPointVelocity(ref pointInfo.m_Info.m_WorldPosition, out globalVel1);
rbc.GetRBElementInteraction().GetRigidBody2().GetGlobalPointVelocity(ref pointInfo.m_Info.m_WorldPosition, out globalVel2);
Vector3 relativeVelocity = globalVel2 - globalVel1;
float aRelVel = Vector3.Dot(pointInfo.m_Info.m_Normal, relativeVelocity);
float diff = aRelVel;
rbsc.m_Target = diff >= 0.0f ? 0.0f : (-diff * restitution);
if (pointInfo.m_Info.m_InitialPenetration < -0.01f)
{
rbsc.m_Target -= pointInfo.m_Info.m_InitialPenetration * m_DepenetrationCoeficient;
}
rbsc.m_Restitution = restitution;
rbsc.m_Normal = pointInfo.m_Info.m_Normal;
rbsc.m_ContactPoint = pointInfo.m_Info.m_WorldPosition;
rbsc.m_Body1LocalPoint = pointInfo.m_Info.m_R0;
rbsc.m_Body2LocalPoint = pointInfo.m_Info.m_R1;
Vector3 cross1 = Vector3.Cross(pointInfo.m_Info.m_R0, pointInfo.m_Info.m_Normal);
Vector3 cross2 = Vector3.Cross(pointInfo.m_Info.m_R1, pointInfo.m_Info.m_Normal);
rbsc.m_Body1LocalPointCrossedNormal = Vector3.Transform(cross1, body1.m_InvertedInertiaTensor);
rbsc.m_Body2LocalPointCrossedNormal = Vector3.Transform(cross2, body2.m_InvertedInertiaTensor);
rbsc.m_StaticFriction = material1.NominalStaticFriction * material2.NominalStaticFriction;
rbsc.m_DynamicFriction = material1.NominalDynamicFriction * material2.NominalDynamicFriction;
rbsc.m_Affection = 0.0f;
if (body1.m_State == MyRBSolverBody.SolverBodyState.SBS_Dynamic)
{
rbsc.m_Affection += Vector3.Dot(rbsc.m_Normal, (rbsc.m_Normal * body1.m_OneOverMass + Vector3.Cross(rbsc.m_Body1LocalPointCrossedNormal, rbsc.m_Body1LocalPoint)));
body1.m_LinearVelocity -= rbsc.m_Normal * (body1.m_OneOverMass * rbsc.m_Magnitude);
body1.m_AngularVelocity -= rbsc.m_Body1LocalPointCrossedNormal * (rbsc.m_Magnitude);
MinerWars.AppCode.Game.Utils.MyUtils.AssertIsValid(body1.m_AngularVelocity);
}
if (body2.m_State == MyRBSolverBody.SolverBodyState.SBS_Dynamic)
{
rbsc.m_Affection += Vector3.Dot(rbsc.m_Normal, (rbsc.m_Normal * body2.m_OneOverMass + Vector3.Cross(rbsc.m_Body2LocalPointCrossedNormal, rbsc.m_Body2LocalPoint)));
body2.m_LinearVelocity -= rbsc.m_Normal * (body2.m_OneOverMass * rbsc.m_Magnitude);
body2.m_AngularVelocity -= rbsc.m_Body2LocalPointCrossedNormal * (rbsc.m_Magnitude);
MinerWars.AppCode.Game.Utils.MyUtils.AssertIsValid(body2.m_AngularVelocity);
}
if (rbo1.ContactModifyNotificationHandler != null)
{
if (!rbo1.ContactModifyNotificationHandler.OnContact(ref rbsc))
{
m_SolverConstaintPool.Deallocate(rbsc);
continue;
}
}
if (rbo2.ContactModifyNotificationHandler != null)
{
if (!rbo2.ContactModifyNotificationHandler.OnContact(ref rbsc))
{
m_SolverConstaintPool.Deallocate(rbsc);
continue;
}
}
m_SolverConstraints.Add(rbsc);
}
}
/// <summary>
/// Updates rigid body position after solver has computed new velocities
/// </summary>
private void UpdatePositions(float dt)
{
foreach (KeyValuePair <MyRigidBody, MyRBSolverBody> kvp in m_SolverBodies)
{
MyRBSolverBody body = kvp.Value;
MyRigidBody rbo = kvp.Key;
if (body.m_State == MyRBSolverBody.SolverBodyState.SBS_Dynamic)
{
rbo.LinearAcceleration = rbo.ExternalLinearAcceleration;
rbo.AngularAcceleration = rbo.ExternalAngularAcceleration;
rbo.ExternalAngularAcceleration = Vector3.Zero;
rbo.ExternalLinearAcceleration = Vector3.Zero;
#if PHYSICS_CHECK
MyCommonDebugUtils.AssertDebug(float.IsNaN(body.m_LinearVelocity.X) == false);
MyCommonDebugUtils.AssertDebug(float.IsNaN(body.m_Matrix.Translation.X) == false);
#endif
rbo.LinearVelocity = body.m_LinearVelocity;
rbo.AngularVelocity = body.m_AngularVelocity;
/*Vector3 pos, scale, scale2;Quaternion rot;
* rbo.Matrix.Decompose(out scale, out rot, out pos);
* body.m_Matrix.Decompose(out scale2, out rot, out pos);
* rbo.Matrix = Matrix.CreateScale(scale) * Matrix.CreateFromQuaternion(rot) * Matrix.CreateTranslation(pos);*/
rbo.Matrix = body.m_Matrix;
rbo.ApplyDamping(dt);
foreach (var el in rbo.GetRBElementList())
{
el.UpdateAABB();
}
}
if (body.m_State == MyRBSolverBody.SolverBodyState.SBS_Kinematic)
{
rbo.LinearAcceleration = Vector3.Zero;
rbo.AngularAcceleration = Vector3.Zero;
rbo.ExternalAngularAcceleration = Vector3.Zero;
rbo.ExternalLinearAcceleration = Vector3.Zero;
rbo.LinearVelocity = body.m_LinearVelocity;
rbo.AngularVelocity = body.m_AngularVelocity;
rbo.SetMatrix(body.m_Matrix);
}
}
for (int i = 0; i < m_SolverConstraints.Count; i++)
{
MyRBSolverConstraint sc = m_SolverConstraints[i];
if (sc.m_Magnitude != 0.0f)
{
sc.m_RBConstraint.m_Magnitude = sc.m_Magnitude;
}
}
}
/// <summary>
/// Rise the OnContact event.
/// </summary>
/// <param name="constraint">The constraint.</param>
/// <returns></returns>
internal bool NotifyContact(ref MyRBSolverConstraint constraint)
{
return OnContact(ref constraint);
}
/// <summary>
/// Called when [contact] with entity.
/// </summary>
/// <param name="constraint">The constraint.</param>
/// <returns></returns>
protected virtual bool OnContact(ref MyRBSolverConstraint constraint)
{
if (OnContactEvent != null)
{
OnContactEvent(constraint.GetOtherEntity(this));
}
return true;
}
/// <summary>
/// Called when contact
/// </summary>
/// <param name="constraint"></param>
/// <returns>false if contact has to be refused</returns>
public virtual bool OnContact(ref MyRBSolverConstraint constraint)
{
return this.Entity.NotifyContact(ref constraint);
}
