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); } }