private void HandleCollision(MoveBall b1, MoveBall b2)
{ //collision detected: the collision has happened anytime between the previous frame and this one.
//find how much time has past since the the collision
MyVector3 relVelocity = MoveBall.RelativeVelocity(b2, b1); // V° rel velocity (const)
MyVector3 relPosition = MoveBall.RelativePosition(b2, b1); // P° rel position at t=0
// relative position P(t) = P° - V° * t
// the collision happens when |P(t)| = 2 * radius
// <P(t)|P(t)> = <(P° - V° * t)|(P° - V° * t)> = 4 * radius^2
// solve the above quadratic eq. for t
float term1 = MyVector3.Dot(relVelocity, relPosition);
float term2 = MyVector3.Dot(relPosition, relPosition);
float term3 = MyVector3.Dot(relVelocity, relVelocity);
float rootSquared = term1 * term1 - term3 * (term2 - 4 * SnookerBall.radius * SnookerBall.radius);
if (rootSquared < 0)
{
rootSquared = 0;
}
float timeAfterCollision = (term1 + Mathf.Sqrt(rootSquared)) / term3;
b1.MoveByVector(b1.GetVelocity().Scale(-timeAfterCollision)); //minus sign because we are moving the ball back in time
b2.MoveByVector(b2.GetVelocity().Scale(-timeAfterCollision)); //minus sign because we are moving the ball back in time
relPosition = MoveBall.RelativePosition(b2, b1); //vector joining the centers of the 2 balls
MyVector3 b1_parallel = b1.GetLinearMomentum().ParallelComponent(relPosition);
MyVector3 b1_perpendicular = b1.GetLinearMomentum().NormalComponent(relPosition);
MyVector3 b2_parallel = b2.GetLinearMomentum().ParallelComponent(relPosition);
MyVector3 b2_perpendicular = b2.GetLinearMomentum().NormalComponent(relPosition);
//the two ball exchange the parallel components of their linear momenta
//this is only valid in the case of the masses being the same
b1.SetLinearMomentum(MyVector3.Add(b1_perpendicular, b2_parallel));
b2.SetLinearMomentum(MyVector3.Add(b2_perpendicular, b1_parallel));
}
//calculate the x velocity resulting from the ball sliding on the floor for a time deltaTime
private MyVector3 HorizontalVelocityAfterSliding(float deltaTime)
{
if (pureRolling)
{
return(velocityPreviousFrame);
}
MyVector3 afterSlidingVel;
MyVector3 horizontal_velocityPreviousFrame =
new MyVector3(velocityPreviousFrame.getX(), 0, velocityPreviousFrame.getZ());
//calculate the variation in the angular momentum and x component of the linear momentum
float arbitraryConst1 = 5f;
MyVector3 frictionDirection = horizontal_velocityPreviousFrame.Scale(-1).UnitVector();
MyVector3 linearMomentumVariation = frictionDirection.Scale(
arbitraryConst1 * floorFriction * mass * deltaTime);
MyVector3 angularMomentumVariation = MyVector3.Cross(
new MyVector3(0, -radius, 0),
linearMomentumVariation);
afterSlidingVel = MyVector3.Add(horizontal_velocityPreviousFrame, linearMomentumVariation.Scale(1 / mass));
angularMomentumCurrentFrame = MyVector3.Add(angularMomentumCurrentFrame, angularMomentumVariation);
MyVector3 contactPointTangentVel = MyVector3.Cross(AngularVelocity(angularMomentumCurrentFrame), new MyVector3(0, -radius, 0));
if (contactPointTangentVel.Magnitude() > afterSlidingVel.Magnitude())
{
pureRolling = true;
//calculate the rolling horizintal velocity
//pure rolling law: speed = (radius) * (angular vel)
float MRsquared_over_I = (mass * radius * radius) / momentOfInertia;
afterSlidingVel =
MyVector3.Subtract(afterSlidingVel.Scale(MRsquared_over_I), contactPointTangentVel).Scale(1 / (1 + MRsquared_over_I));
angularMomentumCurrentFrame = MyVector3.Cross(afterSlidingVel, new MyVector3(0, -1, 0)).Scale(momentOfInertia / radius);
}
return(afterSlidingVel);
}
private void Update()
{
Debug.DrawRay(Vector3.zero, A.transform.forward, Color.red);
Debug.DrawRay(Vector3.zero, B.transform.forward, Color.green);
Vector3 target = MyVector3.Add(A.transform.forward, B.transform.forward);
Resultant.transform.LookAt(target, Vector3.up);
Debug.DrawLine(Vector3.zero, MyVector3.Scale(Resultant.transform.forward, target.magnitude), Color.blue);
}
/// <summary>
/// Constructs a <see cref="MyBoundingSphere" /> that fully contains the given points.
/// </summary>
/// <param name="points">The points that will be contained by the sphere.</param>
/// <param name="start">The start index from points array to start compute the bounding sphere.</param>
/// <param name="count">The count of points to process to compute the bounding sphere.</param>
/// <param name="result">When the method completes, contains the newly constructed bounding sphere.</param>
/// <exception cref="System.ArgumentNullException">points</exception>
/// <exception cref="System.ArgumentOutOfRangeException">
/// start
/// or
/// count
/// </exception>
public static void FromPoints(MyVector3[] points, int start, int count, out MyBoundingSphere result)
{
if (points == null)
{
throw new ArgumentNullException("points");
}
// Check that start is in the correct range
if (start < 0 || start >= points.Length)
{
throw new ArgumentOutOfRangeException("start", start, string.Format("Must be in the range [0, {0}]", points.Length - 1));
}
// Check that count is in the correct range
if (count < 0 || (start + count) > points.Length)
{
throw new ArgumentOutOfRangeException("count", count, string.Format("Must be in the range <= {0}", points.Length));
}
var upperEnd = start + count;
//Find the center of all points.
MyVector3 center = MyVector3.Zero;
for (int i = start; i < upperEnd; ++i)
{
MyVector3.Add(ref points[i], ref center, out center);
}
//This is the center of our sphere.
center /= (float)count;
//Find the radius of the sphere
float radius = 0f;
for (int i = start; i < upperEnd; ++i)
{
//We are doing a relative distance comparison to find the maximum distance
//from the center of our sphere.
float distance;
MyVector3.DistanceSquared(ref center, ref points[i], out distance);
if (distance > radius)
{
radius = distance;
}
}
//Find the real distance from the DistanceSquared.
radius = (float)Math.Sqrt(radius);
//Construct the sphere.
result.Center = center;
result.Radius = radius;
}
private void FixedUpdate()
{
//simulation starts upon pressing the spacebar
if (Input.GetKeyDown(KeyCode.Space) == true)
{
startSimulation = true;
}
if (!startSimulation)
{
return;
}
float t = Time.fixedDeltaTime;
MyVector3 previousLinearMomentum = linearMomentum;
MyVector3 linearMomentumVariation = linearMomentum.UnitVector().Scale(
-SnookerBall.NormalForce * SnookerBall.frictionCoeff * t);
if (linearMomentumVariation.Magnitude() >= linearMomentum.Magnitude())
{//the decrease in linear momentum is bigger than the linear momentum itself
linearMomentum = MyVector3.Zero();
}
else
{
linearMomentum = MyVector3.Add(linearMomentum, linearMomentumVariation);
}
MyVector3 avgLinearMomentum = MyVector3.Add(linearMomentum, previousLinearMomentum).Scale(0.5f);
MyVector3 avgVelocity = avgLinearMomentum.Scale(1 / SnookerBall.mass);
MyVector3 newPosition = MyVector3.Add(position, avgVelocity.Scale(t));
foreach (Plane plane in cushions)
{
float distanceFromPlane = newPosition.DistanceFromPlane(plane) - SnookerBall.radius;
if (distanceFromPlane < 0)
{//the ball has gone beyond the boundary
//collision detected: the collision has happened anytime between the previous frame and this one.
//find how much time has past since the collision
float impactTime =
ImpactTime(
MyVector3.Dot(previousLinearMomentum, plane.Normal()),
-MyVector3.Dot(avgLinearMomentum.UnitVector(), plane.Normal()) * SnookerBall.NormalForce * SnookerBall.frictionCoeff,
position.DistanceFromPlane(plane) - SnookerBall.radius);
linearMomentumVariation = previousLinearMomentum.UnitVector().Scale(
-SnookerBall.NormalForce * SnookerBall.frictionCoeff * impactTime);
//reflect the linear momentum along the normal of the plane
linearMomentum = MyVector3.Add(previousLinearMomentum, linearMomentumVariation).Reflect(plane.Normal());
//position ball just within the boundaries
newPosition = MyVector3.Add(newPosition, plane.Normal().Scale(-distanceFromPlane));
}
}
position = newPosition;
MoveGameObject();
}
private void SetLinearMovementVariables()
{
rotationPosition = new MyVector3_Cylindrical(
rotationPosition.getRadius(),
directionConcesutiveWaypoints.getAzmuthalRad(), //place ball exctly between the current and the next waypoint
rotationPosition.getY());
rotating = false;
position = MyVector3.Add(rotationPosition.ToCartesian(), waypoints_position[target]);
MakeRed(target);
target = NextTarget();
MakeGreen(target);
MakeYellow(NextTarget());
movementDirection = MyVector3.DirectionalUnitVector(position, waypoints_position[target]);
}
public void FixedUpdate()
{
if (rotating)
{//the ball is rotating around a waypoint
//angle ball->current waypoint->next waypoint at the end of the previous update (clockwise winding)
float prevAngle = MyVector3_Cylindrical.DeltaAngleRadiants(rotationPosition, directionConcesutiveWaypoints);
int sign = clockwise ? -1 : 1;
float dAngle = angularVelocity * Time.fixedDeltaTime * sign;
rotationPosition.RotateByAngle(dAngle);
//angle ball->current waypoint->next waypoint at the current update
float angle = MyVector3_Cylindrical.DeltaAngleRadiants(rotationPosition, directionConcesutiveWaypoints);
if ((!clockwise && prevAngle > Mathf.PI && angle < Mathf.PI) ||
(clockwise && prevAngle < Mathf.PI && angle > Mathf.PI) ||
dAngle > Mathf.PI)
{//the ball has moved past the next movement direction on this frame
SetLinearMovementVariables();
}
else
{//the ball has NOT crossed the next movement direction on this frame
position = MyVector3.Add(rotationPosition.ToCartesian(), waypoints_position[target]);
}
}
else
{
if (MyVector3.CloserThanRadius(position, waypoints_position[target], rotationRadius))
{//the ball is closer to the waypoint than the specified radius
SetRotationVariables();
}
else
{
MyVector3 deltaPos = movementDirection.Scale(Time.fixedDeltaTime * velocity);
float distanceFromWaypoint = MyVector3.Subtract(waypoints_position[target], position).Magnitude();
if (distanceFromWaypoint - deltaPos.Magnitude() <= rotationRadius)
{//the ball has moved closer than the specified radius on this frame
position = MyVector3.Subtract(waypoints_position[target], movementDirection.Scale(rotationRadius));
SetRotationVariables();
}
else
{//the ball has NOT moved closer than the specified radius on this frame
position = MyVector3.Add(position, deltaPos);
}
}
}
//place object in the scene
MoveGameObject();
}
public void Calculate()
{
try
{//parse strings into floating point numbers
float v1_x = Single.Parse(v1_inputs[0].text);
float v1_y = Single.Parse(v1_inputs[1].text);
float v1_z = Single.Parse(v1_inputs[2].text);
float v2_x = Single.Parse(v2_inputs[0].text);
float v2_y = Single.Parse(v2_inputs[1].text);
float v2_z = Single.Parse(v2_inputs[2].text);
float num = Single.Parse(scalar_input.text);
v1 = new MyVector3(v1_x, v1_y, v1_z);
v2 = new MyVector3(v2_x, v2_y, v2_z);
scalar = num;
}
catch (Exception e)
{//parsing failed
Debug.LogError(e.Message);
v1_inputs[0].text = "";
v1_inputs[1].text = "";
v1_inputs[2].text = "";
v2_inputs[0].text = "";
v2_inputs[1].text = "";
v2_inputs[2].text = "";
scalar_input.text = "";
return;
}
//print vectors in the scene
add.text = MyVector3.Add(v1, v2).Print();
subtract.text = MyVector3.Subtract(v1, v2).Print();
scale.text = v1.Scale(scalar).Print();
dot.text = MyVector3.Dot(v1, v2).ToString("0.00");
magnitude.text = v1.Magnitude().ToString("0.00");
unitVect.text = v1.UnitVector().Print();
reflectX.text = v1.ReflectX().Print();
reflectY.text = v1.ReflectY().Print();
reflectZ.text = v1.ReflectZ().Print();
zero.text = MyVector3.Zero().Print();
}
//move the ball by the specified vector
public void MoveByVector(MyVector3 v)
{
position = MyVector3.Add(v, position);
}
private void FixedUpdate()
{
//simulation start upon press of the spacebar
if (Input.GetKeyDown(KeyCode.Space) == true)
{
startSimulation = true;
}
if (!startSimulation)
{
return;
}
if (fullStop)
{
return; //simulation ended
}
float t = Time.fixedDeltaTime;
if (stopBouncing)
{
if (pureRolling)
{//case 1: the ball is rolling on the floor without sliding
float dumping = 1 -
(5 - 4 / (1 + mass)) *
(floorFriction / (4 * floorFriction + 6)) *
(1 + 49 / (1 + 10 * Mathf.Abs(velocityPreviousFrame.Magnitude()))) *
t; //arbitrary dumping factor. No real physical meaning.
velocityCurrentFrame = velocityPreviousFrame.Scale(dumping);
if (velocityCurrentFrame.Magnitude() < 0.01f)
{//the velocity is low enough to be neglectable
velocityCurrentFrame = MyVector3.Zero();
fullStop = true;
}
//calculate the avarage velocity between two consecutive frames and use it to calculate the new position
MyVector3 avgVelocity = MyVector3.Lerp(velocityPreviousFrame, velocityCurrentFrame, 0.5f);
MyVector3 displacement = avgVelocity.Scale(t);
position = MyVector3.Add(position, displacement);
MoveGameObject();
angularMomentumCurrentFrame = MyVector3.Cross(velocityCurrentFrame, new MyVector3(0, -1, 0)).Scale(momentOfInertia / radius);
RotateGameObject(t);
velocityPreviousFrame = velocityCurrentFrame;
angularMomentumPreviousFrame = angularMomentumCurrentFrame;
}
else
{//case 2: the ball is rolling on the floor with sliding
velocityCurrentFrame = HorizontalVelocityAfterSliding(t);
//calculate the avarage velocity between two consecutive frames and use it to calculate the new position
MyVector3 avgVelocity = MyVector3.Lerp(velocityPreviousFrame, velocityCurrentFrame, 0.5f);
MyVector3 displacement = avgVelocity.Scale(t);
position = MyVector3.Add(position, displacement);
MoveGameObject();
//angularMomentumCurrentFrame set in the X_VelocityAfterSliding() function
RotateGameObject(t);
velocityPreviousFrame = velocityCurrentFrame;
angularMomentumPreviousFrame = angularMomentumCurrentFrame;
}
}
else
{//the ball is still bouncing
MyVector3 deltaVel = gravity.Scale(t);
velocityCurrentFrame = MyVector3.Add(velocityPreviousFrame, deltaVel);
//calculate the avarage velocity between two consecutive frames and use it to calculate the new position
MyVector3 avgVelocity = MyVector3.Lerp(velocityPreviousFrame, velocityCurrentFrame, 0.5f);
MyVector3 displacement = avgVelocity.Scale(t);
MyVector3 newPosition = MyVector3.Add(position, displacement);
float distanceFromGround = newPosition.DistanceFromPlane(ground) - radius;
if (distanceFromGround < 0)
{ //case 3: the ball is bouncing and has partially fallen balow the plane of the ground
float impactVel_y = Y_ImpactVelocity();
float velocityCurrentFrame_y = impactVel_y * restitution; //upwards speed after bounce
if (velocityCurrentFrame_y < tresholdStopBouncing)
{ //the speed after the bounce is small enough to be ignored
stopBouncing = true;
velocityCurrentFrame_y = 0;
}
MyVector3 horzVelocityCurrentFrame = HorizontalVelocityAfterSliding(t + 0.001f * impactVel_y + 8 * t * (1 - 1 / Mathf.Sqrt(1 + mass)));
//position ball on top of the ground
position = MyVector3.Add(newPosition, ground.Normal().Scale(-distanceFromGround));
MoveGameObject();
//angularMomentumCurrentFrame set in the X_VelocityAfterSliding() function
RotateGameObject(t);
angularMomentumPreviousFrame = angularMomentumCurrentFrame;
velocityPreviousFrame = new MyVector3(horzVelocityCurrentFrame.getX(), velocityCurrentFrame_y, horzVelocityCurrentFrame.getZ());
}
else
{//case 4: the ball is bouncing and is in mid-air
velocityPreviousFrame = velocityCurrentFrame;
position = newPosition;
MoveGameObject();
RotateGameObject(t);
}
}
}
