private void Update()
{
if (Input.GetMouseButtonDown(0) == true)
{
Vector2 pos = Camera.main.ScreenToWorldPoint(Input.mousePosition);
RaycastHit2D[] hits = Physics2D.RaycastAll(pos, Vector2.zero, float.PositiveInfinity);
if (simArray[0].UnderSimulation == true)
{
return;
}
if (hits.ToList().Exists(_ => _.collider.gameObject == this.gameObject))
{
float eps1 = 0.001f;
float eps2 = 0.03f;
bool isValid = true;
foreach (RigidBodySimulation lhs in simArray)
{
foreach (RigidBodySimulation rhs in simArray)
{
float depth;
Vector2 deepestPoint, direction;
if (lhs != rhs)
{
CustomPhysics.GetPenetrationDepth(lhs.ToPointArray(), rhs.ToPointArray(), out depth, out deepestPoint, out direction);
if (Mathf.Abs(depth) > eps1)
{
Debug.Log("failed at " + lhs.name + ", " + rhs.name);
isValid = false;
goto there;
}
}
}
}
foreach (RigidBodySimulation sim in simArray)
{
if (sim.LinearVelocity.magnitude > eps2 || Mathf.Abs(sim.AngularVelocity) > eps2)
{
Debug.Log("failed at " + sim.name);
Debug.Log("linvel " + sim.LinearVelocity.magnitude.ToString() + ", angvel " + sim.AngularVelocity.ToString());
isValid = false;
goto there;
}
}
there:
if (isValid == true)
{
foreach (RigidBodySimulation sim in simArray)
{
sim.StartSimulation();
}
}
else
{
overlapWarn.StartWarn();
}
}
}
}
void FixedUpdate()
{
if (SimulationHalted)
{
return;
}
if (!UnderSimulation && dragging)
{
return;
}
if (!UnderSimulation && !IsDragable)
{
return;
}
//if (underSimulation == false)
// return;
float dt = Time.fixedDeltaTime;
Vector2 worldForce = Vector2.zero;
float worldTorque = 0;
//gravity
Vector2 gravity = mass * gravityVector;
worldForce += gravity;
//penalty method
float degreeAngle = transform.rotation.eulerAngles.z;
RigidBodySimulation[] simArray = GameObject.FindObjectsOfType <RigidBodySimulation>();
// Make some overlapboxall
bool collided = false;
foreach (RigidBodySimulation sim in simArray)
{
if (sim == this)
{
continue;
}
if (this.IsEndDomino == false && sim.gameObject.GetComponent <TargetCube>() != null)
{
continue;
}
float depth;
Vector2 deepestPoint;
Vector2 direction;
CustomPhysics.GetPenetrationDepth(
this.ToPointArray(), sim.ToPointArray(),
out depth, out deepestPoint, out direction);
if (depth != 0)
{
if (!UnderSimulation)
{
if (sim.dragging)
{
return;
}
LinearVelocity = Vector2.zero;
AngularVelocity = 0;
sim.LinearVelocity = Vector2.zero;
sim.AngularVelocity = 0;
transform.Translate(transform.InverseTransformDirection((depth) * direction));
return;
}
else
{
if (tc != null && sim.IsEndDomino == true)
{
tc.Collided();
}
Vector2 f = penaltyConstant * direction * depth;
worldForce += f;
Vector2 r = deepestPoint - (Vector2)transform.position;
if (Mathf.Abs(worldTorque) > 0.01 || Mathf.Abs(r.x * f.y - r.y * f.x) > 0.01)
{
worldTorque += r.x * f.y - r.y * f.x;
}
collided = true;
float dotproduct = Vector2.Dot(LinearVelocity, direction);
Vector2 directionwiseVelocity = dotproduct * direction;
LinearVelocity =
(LinearVelocity - directionwiseVelocity) * (1 - linearDampingCoeffX * dt) //perpendicular to normal
+ directionwiseVelocity * (1 - linearDampingCoeffY * dt); //normal
}
//CustomPhysics.DebugVector2(deepestPoint);
}
}
if (collided == true)
{
AngularVelocity *= (1 - angularDampingCoeff * dt);
}
LinearVelocity += dt * AngularVelocity * new Vector2(LinearVelocity.y, -LinearVelocity.x);
LinearVelocity += dt * worldForce / mass; //normal term
AngularVelocity += dt * worldTorque / inertia;
LinearVelocity *= (1 - linearDrag * dt);
AngularVelocity *= (1 - angularDrag * dt);
if (!UnderSimulation)
{
AngularVelocity = 0;
}
if (IsStatic == false)
{
transform.Translate(transform.InverseTransformDirection(LinearVelocity) * dt);
transform.Rotate(0, 0, AngularVelocity * dt * 180 / Mathf.PI);
}
}