public void FixedUpdate() { if (gameObject.enabled && enabled && !isKinematic) { Vector2 prevVel = velocity; velocity += pendingVelocity; pendingVelocity = Vector2.zero; if (useGravity) { velocity += Physics.gravity * MainWindow.fixedDeltaTime; } float da = dragAdd + pendingDragAdd; pendingDragAdd = 0; Vector2 dragVel = Vector2.zero; if (da > 0) { dragVel += Vector2.ClampMagnitude(velocity, da * MainWindow.fixedDeltaTime); } float dm = MyMath.Clamp(pendingDragMultiply + dragMul, 0, 1); pendingDragMultiply = 0; if (dm > 0) { dragVel += velocity * dm * MainWindow.fixedDeltaTime; } dragVel = Vector2.ClampMagnitude(dragVel, velocity.magnitude); velocity -= dragVel; Vector2 moveDir = 0.5f * (prevVel + velocity) * MainWindow.fixedDeltaTime + pendingMovement; pendingMovement = Vector2.zero; float moveDirMagnitude = moveDir.magnitude; float prevAngularSpeed = angularSpeed; angularSpeed += pendingAngularSpeed; float normalizedAngularSpeed = MyMath.Normalize(angularSpeed); float angularSpeedMag = Math.Abs(angularSpeed); pendingAngularSpeed = 0; float ada = angularDragAdd + pendingAngularDragAdd; pendingAngularDragAdd = 0; float angularDragVel = 0; if (ada > 0) { angularDragVel += MyMath.Clamp(angularSpeedMag, 0, ada * MainWindow.fixedDeltaTime); } float adm = MyMath.Clamp(pendingAngularDragMul + angularDragMul, 0, 1); pendingAngularDragMul = 0; if (adm > 0) { angularDragVel += angularSpeedMag * adm * MainWindow.fixedDeltaTime; } angularDragVel = MyMath.Clamp(angularDragVel, 0, angularSpeedMag); angularSpeed -= normalizedAngularSpeed * angularDragVel; angularSpeed = MyMath.Clamp(angularSpeed, -20, 20); float rotation = 0.5f * (prevAngularSpeed + angularSpeed) * MainWindow.fixedDeltaTime + pendingRotation; pendingRotation = 0; float startingMovedirmag = moveDirMagnitude; if (moveDirMagnitude > 0 || rotation != 0) { Vector2 normalizedMoveDir = moveDir / moveDirMagnitude; float rotationMag = Math.Abs(rotation); Vector2 moveDone = Vector2.zero; float normalizedRotation = MyMath.Normalize(rotation); gameObject.transform.position += moveDir; if (collider == null) { gameObject.transform.rotation += rotation; return; } if (rotation != 0) { gameObject.transform.RotateAround(collider.pivot, rotation, true); } if (!collider.isTrigger) { List <Collision> cols = collider.GetCollisions(); if (cols != null) { Vector2 worldPivot = gameObject.transform.LocalToWorldPoint(collider.pivot); Vector2 accNormal = Vector2.zero; float accRad = 0; float sumArea = 0; int len = cols.Count; float sumLength = 0; for (int i = len - 1; i >= 0; i--) { Collision n = new Collision(); n.normal = -cols [i].normal; n.area = cols [i].area; n.isCollision = true; n.rejection = cols [i].rejection; n.additionalFloats = cols [i].additionalFloats; n.point = cols [i].point; n.collider = collider; cols [i].collider.gameObject.GetCollision(n); gameObject.GetCollision(cols [i]); cols [i].additionalFloats = new float[3]; float dot = Vector2.Dot(normalizedMoveDir, -cols [i].normal); Vector2 r = (cols [i].point - worldPivot).normalized; float sin = Vector2.Cross(r, cols [i].normal); if (dot <= 0 && sin >= 0) { cols.RemoveAt(i); } else { cols [i].additionalFloats [0] = dot; //if (dot > 0) sumArea += cols [i].area; cols [i].additionalFloats [1] = Vector2.Dot(Vector2.Rotate(r, MyMath.FloatPIPer2) * normalizedRotation, cols [i].normal); if (cols [i].additionalFloats [1] >= 0) { cols [i].additionalFloats [2] = Vector2.Distance(worldPivot, cols [i].point); sumLength += cols [i].area * cols [i].additionalFloats [2]; } else { cols [i].additionalFloats [2] = 0; } } } if (cols.Count > 0) { moveDir = normalizedMoveDir * moveDirMagnitude; Vector2 rejAng = Vector2.zero; foreach (Collision s in cols) { Vector2 projVel = Vector2.zero; Vector2 projAng = Vector2.zero; float ang = 0; float retractAng = 0; Vector2 r = s.point - worldPivot; float rMag = r.magnitude; float k = inertia / mass / rMag / rMag; if (s.area > 0) { if (s.additionalFloats [0] > 0) { projVel += Vector2.Project(moveDir, -s.normal) * s.area / sumArea; } if (rotationMag > 0 && sumLength > 0 && s.additionalFloats [2] > 0) { ang = Math.Abs(rotation) * s.additionalFloats [2] * s.area / sumLength; Vector2 velAng = Vector2.Rotate(r, MyMath.FloatPIPer2).normalized *normalizedRotation *s.additionalFloats [2] * ang; projAng = Vector2.Project(velAng, -s.normal); rejAng = Vector2.Reject(velAng, -s.normal); retractAng = projAng.magnitude / s.additionalFloats [2]; } if (Vector2.HasNaN(projAng)) { gameObject.Throw("projAng has nan"); } } Vector2 proj = projVel + projAng; Vector2 vel = proj / MainWindow.fixedDeltaTime; float restCoef = restitutionCoef; float fricCoef = frictionCoef; Vector2 lostVelocity = Vector2.zero; float mass2 = s.collider.gameObject.rigidbody.mass; if (s.collider.gameObject.rigidbody != null) { if (restitutionMultiply) { restCoef = restitutionCoef * s.collider.gameObject.rigidbody.restitutionCoef; } else { restCoef = (restitutionCoef + s.collider.gameObject.rigidbody.restitutionCoef) / 2; } if (frictionMultiply) { fricCoef = frictionCoef * s.collider.gameObject.rigidbody.frictionCoef; } else { fricCoef = (frictionCoef + s.collider.gameObject.rigidbody.frictionCoef) / 2; } //Vector2 givenVelocity = mass * vel / (mass * (1 - restCoef) + mass2); //lostVelocity = vel * (mass * restCoef + mass2) / (mass + mass2); Vector2 givenVelocity = mass * vel * (1 + restCoef) / (mass + mass2); lostVelocity = mass2 * vel * (1 + restCoef) / (mass + mass2); if (!s.collider.gameObject.rigidbody.isKinematic) { s.collider.gameObject.rigidbody.AddVelocity(givenVelocity, s.point); } else { lostVelocity += givenVelocity; } AddVelocity(-lostVelocity, s.point); float coef = (lostVelocity.magnitude / vel.magnitude); } else { lostVelocity = vel * (1 + restCoef); AddVelocity(-lostVelocity, s.point); } float frictionVelocityMag = lostVelocity.magnitude * fricCoef; if (frictionVelocityMag > 0) { Vector2 frictionVelocity = Vector2.ClampMagnitude(-(velocity + pendingVelocity + rejAng / MainWindow.fixedDeltaTime), lostVelocity.magnitude * fricCoef); if (s.collider.gameObject.rigidbody != null && !s.collider.gameObject.rigidbody.isKinematic) { s.collider.gameObject.rigidbody.AddImpulse(-frictionVelocity * mass, s.point); } accRad += Vector2.Project(frictionVelocity, rejAng).magnitude *MainWindow.fixedDeltaTime / rMag; AddVelocity(frictionVelocity, s.point); } float diff = proj.magnitude - s.rejection; if (diff > 0) { float projangMag = projAng.magnitude; if (projangMag > 0) { projVel = projVel.normalized * (projVel.magnitude + diff / 2); projAng = projAng.normalized * (projangMag + diff / 2); ang = (projAng + rejAng).magnitude / s.additionalFloats [2]; retractAng = projAng.magnitude / s.additionalFloats [2]; } else { projVel = projVel.normalized * (projVel.magnitude + diff); } } accRad += retractAng; accNormal += projVel; } } gameObject.transform.position -= accNormal; gameObject.transform.RotateAround(collider.pivot, normalizedRotation * -accRad, true); //angularSpeed += normalizedRotation * -accRad/MainWindow.fixedDeltaTime; } } } else { /*List<Collision> cols = collider.GetCollisions (); * if (cols != null) { * foreach (Collision s in cols) { * Vector2 proj = -s.normal * s.rejection; * if (s.collider.gameObject.rigidbody != null && !s.collider.gameObject.rigidbody.isKinematic) { * s.collider.gameObject.transform.position += proj / 2; * gameObject.transform.position -= proj / 2; * } else { * gameObject.transform.position -= proj; * } * * } * }*/ } } }
public static Vector2 Lerp(Vector2 a, Vector2 b, float t) { return(a + (b - a) * MyMath.Clamp(t, 0, 1)); }