private void InelasticCollision(IForwardMovablePhysicalEntity target, IForwardMovablePhysicalEntity source,
float diffAngle)
{
Vector2 decomposedSpeedTarget = Utils.DecomposeSpeed(target.ForwardSpeed, target.Direction, diffAngle);
Vector2 decomposedSpeedSource = Utils.DecomposeSpeed(source.ForwardSpeed, source.Direction, diffAngle);
float v1 = decomposedSpeedTarget.Y;
float v2 = decomposedSpeedSource.Y;
float m1 = target.Weight;
float m2 = source.Weight;
float newSpeedTargetAndSource = (m1 * v1 + m2 * v2) / (m1 + m2);
decomposedSpeedTarget.Y = newSpeedTargetAndSource;
decomposedSpeedSource.Y = newSpeedTargetAndSource;
Tuple <float, float> composedSpeedTarget = Utils.ComposeSpeed(decomposedSpeedTarget, diffAngle);
Tuple <float, float> composedSpeedSource = Utils.ComposeSpeed(decomposedSpeedSource, diffAngle);
float targetSpeed = composedSpeedTarget.Item1;
float sourceSpeed = composedSpeedSource.Item1;
target.ForwardSpeed = targetSpeed > m_collisionChecker.MaximumGameObjectSpeed
? m_collisionChecker.MaximumGameObjectSpeed
: targetSpeed;
target.Direction = composedSpeedTarget.Item2;
source.ForwardSpeed = sourceSpeed > m_collisionChecker.MaximumGameObjectSpeed
? m_collisionChecker.MaximumGameObjectSpeed
: sourceSpeed;
source.Direction = composedSpeedSource.Item2;
}
private void ResolveCollisionOfTwoIForwardMovablePe(IForwardMovablePhysicalEntity pe0, IForwardMovablePhysicalEntity pe1)
{
if (pe0 == pe1)
{
return;
}
Vector2 diff = (pe0.Position - pe1.Position);
Vector2 normalDiff = Vector2.Normalize(diff);
float diffAngle = (float)Math.Atan2(normalDiff.X, -normalDiff.Y);
if (pe0.ElasticCollision || pe1.ElasticCollision)
{
ElasticCollision(pe0, pe1, diffAngle);
}
else if (pe0.InelasticCollision || pe1.InelasticCollision)
{
if (pe0.InelasticCollision)
{
InelasticCollision(pe0, pe1, diffAngle);
}
}
else
{
pe0.ForwardSpeed = 0;
}
}
/// <summary>
/// Search for position close to obstacle in given direction. Must be on stable position when starting search.
/// </summary>
/// <param name="physicalEntity"></param>
/// <param name="initialSpeed"></param>
/// <param name="direction"></param>
private void TileFreePositionBinarySearch(IForwardMovablePhysicalEntity physicalEntity, float initialSpeed,
float direction)
{
float speed = initialSpeed;
Vector2 lastNotColliding = physicalEntity.Position;
bool goForward = true;
for (int i = 0; i < BINARY_SEARCH_ITERATIONS; i++)
{
if (goForward)
{
m_movementPhysics.Shift(physicalEntity, speed, direction);
}
else
{
m_movementPhysics.Shift(physicalEntity, -speed, direction);
}
bool colliding = m_collisionChecker.CollidesWithTile(physicalEntity);
if (!colliding)
{
lastNotColliding = physicalEntity.Position;
}
speed = speed / 2;
goForward = !colliding;
}
physicalEntity.Position = lastNotColliding;
}
private void BounceFromTile(IForwardMovablePhysicalEntity physicalEntity, float speed)
{
TileFreePositionBinarySearch(physicalEntity, physicalEntity.ForwardSpeed, physicalEntity.Direction);
Vector2 originalPosition = physicalEntity.Position;
float originalDirection = physicalEntity.Direction;
float maxDistance = 0;
float bestDirection = MathHelper.WrapAngle(originalDirection + (float)Math.PI);
// emergency reverse direction if either bounce fails
// candidate directions to move
var candidateDirections = new List <float>
{
MathHelper.WrapAngle(2f * MathHelper.Pi - physicalEntity.Direction),
MathHelper.WrapAngle(3f * MathHelper.Pi - physicalEntity.Direction)
};
// search for direction of longest move
foreach (float newDirection in candidateDirections)
{
TileFreePositionBinarySearch(physicalEntity, speed, newDirection);
float distance = Vector2.Distance(originalPosition, physicalEntity.Position);
if (maxDistance < distance)
{
maxDistance = distance;
bestDirection = newDirection;
}
physicalEntity.Position = originalPosition;
}
physicalEntity.Direction = bestDirection;
}
public void TestMoveForward(float speed, float direction)
{
var startingPosition = new Vector2(5, 5);
var movableMock = new Mock <IForwardMovablePhysicalEntity>();
/*movableMock.Setup(x => x.Position).Returns(startingPosition);
* movableMock.Setup(x => x.ForwardSpeed).Returns(speed);
* movableMock.Setup(x => x.Direction).Returns(direction);*/
movableMock.SetupAllProperties();
movableMock.Object.Position = startingPosition;
movableMock.Object.ForwardSpeed = speed;
movableMock.Object.Direction = MathHelper.ToRadians(direction);
IForwardMovablePhysicalEntity movable = movableMock.Object;
m_movementPhysics.Move(movable);
if (speed == 0f)
{
Assert.True(movable.Position == startingPosition);
}
else if (speed == 1f)
{
switch ((int)direction)
{
case 0:
Assert.True(CompareVectors(movable.Position, new Vector2(5, 6)));
break;
case 90:
Assert.True(CompareVectors(movable.Position, new Vector2(4, 5)));
break;
case 180:
Assert.True(CompareVectors(movable.Position, new Vector2(5, 4)));
break;
case 270:
Assert.True(CompareVectors(movable.Position, new Vector2(6, 5)));
break;
case 135:
Assert.True(movable.Position.X < 4.5f && movable.Position.Y < 4.5f);
break;
}
}
else if (speed == -1f)
{
if (direction == 135)
{
Assert.True(movable.Position.X > 5.5f && movable.Position.Y > 5.5f);
}
}
}
public void SetAvatarMotion(IAvatar avatar)
{
IForwardMovablePhysicalEntity physicalEntity = avatar.PhysicalEntity;
Debug.Assert(physicalEntity != null, "physicalEntity != null");
physicalEntity.ForwardSpeed = avatar.DesiredSpeed * MaximumSpeed;
physicalEntity.RotationSpeed = avatar.DesiredLeftRotation * MathHelper.ToRadians(MaximumRotationSpeed);
avatar.Rotation += physicalEntity.RotationSpeed;
}
private void ElasticCollision(IForwardMovablePhysicalEntity target, IForwardMovablePhysicalEntity source,
float diffAngle)
{
Vector2 decomposedSpeedTarget = Utils.DecomposeSpeed(target.ForwardSpeed, target.Direction, diffAngle);
Vector2 decomposedSpeedSource = Utils.DecomposeSpeed(source.ForwardSpeed, source.Direction, diffAngle);
float v1 = decomposedSpeedTarget.Y;
float v2 = decomposedSpeedSource.Y;
float m1 = target.Weight;
float m2 = source.Weight;
float originalEnergy = target.ForwardSpeed * target.ForwardSpeed * m1 +
source.ForwardSpeed * source.ForwardSpeed * m2;
double p = m1 * v1 + m2 * v2; // P = momentum
double d = 4.0 * m1 * m1 * m2 * m2 * (v1 - v2) * (v1 - v2); // D = determinant
float v2A = (float)((2 * p * m2 + Math.Sqrt(d)) / (2 * (m1 * m2 + m2 * m2)));
float v2B = (float)((2 * p * m2 - Math.Sqrt(d)) / (2 * (m1 * m2 + m2 * m2)));
float newSourceSpeed = v2A;
if (Math.Abs(v2A - v2) < Math.Abs(v2B - v2)) // one of the results is identity
{
newSourceSpeed = v2B;
}
float newTargetSpeed = (m1 * v1 + m2 * v2 - m2 * newSourceSpeed) / m1;
/*
* float E2C = v1 * v1 * m1 + v2 * v2 * m2;
* float E2D = newTargetSpeed * newTargetSpeed * m1 + newSourceSpeed * newSourceSpeed * m2;
* Debug.Assert(Math.Abs(E2C - E2D) < 0.000001); // energy conservation check */
decomposedSpeedTarget.Y = newTargetSpeed;
decomposedSpeedSource.Y = newSourceSpeed;
Tuple <float, float> composedSpeedTarget = Utils.ComposeSpeed(decomposedSpeedTarget, diffAngle);
Tuple <float, float> composedSpeedSource = Utils.ComposeSpeed(decomposedSpeedSource, diffAngle);
float targetSpeed = composedSpeedTarget.Item1;
float sourceSpeed = composedSpeedSource.Item1;
float finalEnergy = targetSpeed * targetSpeed * m1 + sourceSpeed * sourceSpeed * m2;
Debug.Assert(Math.Abs(originalEnergy - finalEnergy) < 0.01);
target.ForwardSpeed = targetSpeed > m_collisionChecker.MaximumGameObjectSpeed
? m_collisionChecker.MaximumGameObjectSpeed
: targetSpeed;
target.Direction = composedSpeedTarget.Item2;
source.ForwardSpeed = sourceSpeed > m_collisionChecker.MaximumGameObjectSpeed
? m_collisionChecker.MaximumGameObjectSpeed
: sourceSpeed;
source.Direction = composedSpeedSource.Item2;
}
private void ResolveCollisionOfTwo(IForwardMovablePhysicalEntity pe0, IForwardMovablePhysicalEntity pe1)
{
if (pe0 == pe1)
{
Debug.Assert(false);
}
Vector2 diff = (pe0.Position - pe1.Position);
Vector2 orthogonal = new Vector2(-diff.Y, diff.X);
Vector2 normalOrthogonal = Vector2.Normalize(orthogonal);
float orthogonalAngle0 = (float)Math.Atan2(normalOrthogonal.Y, normalOrthogonal.X);
float orthogonalAngle1 = (float)Math.Atan2(-normalOrthogonal.Y, -normalOrthogonal.X);
CollidesWithLine(pe0, orthogonalAngle0);
CollidesWithLine(pe1, orthogonalAngle1);
}
private void FindTileFreeDirection(IForwardMovablePhysicalEntity physicalEntity, float timeLeft)
{
if (physicalEntity.ElasticCollision)
{
BounceFromTile(physicalEntity, timeLeft);
}
else if (physicalEntity.InelasticCollision)
{
SlideAroundTile(physicalEntity, timeLeft);
}
else
{
physicalEntity.ForwardSpeed = 0;
}
}
private void SlideAroundTile(IForwardMovablePhysicalEntity physicalEntity, float time)
{
Vector2 freePosition = physicalEntity.Position;
float directionRads = physicalEntity.Direction;
float speed = time * physicalEntity.ForwardSpeed;
float xSpeed = (float)Math.Sin(directionRads) * speed;
float ySpeed = (float)Math.Cos(directionRads) * speed;
// position before move
// try to move orthogonally left/right and up/down
TileFreePositionBinarySearch(physicalEntity, xSpeed, X_DIRECTION);
Vector2 xPosition = new Vector2(physicalEntity.Position.X, physicalEntity.Position.Y);
physicalEntity.Position = freePosition;
// try to move orthogonally up/down and left/right; reset position first
TileFreePositionBinarySearch(physicalEntity, ySpeed, Y_DIRECTION);
Vector2 yPosition = new Vector2(physicalEntity.Position.X, physicalEntity.Position.Y);
physicalEntity.Position = freePosition;
float distanceX = Vector2.Distance(freePosition, xPosition);
float distanceY = Vector2.Distance(freePosition, yPosition);
if (Math.Abs(distanceX) < NEGLIGIBLE_DISTANCE &&
Math.Abs(distanceY) < NEGLIGIBLE_DISTANCE)
{
physicalEntity.ForwardSpeed = 0;
return;
}
// farther position is chosen
if (distanceX > distanceY)
{
Vector2 diff = xPosition - freePosition;
physicalEntity.Direction = -(float)Math.Atan2(diff.X, diff.Y);
physicalEntity.ForwardSpeed = Math.Abs(xSpeed) / time;
}
else
{
Vector2 diff = yPosition - freePosition;
physicalEntity.Direction = (float)Math.Atan2(diff.X, diff.Y);
physicalEntity.ForwardSpeed = Math.Abs(ySpeed) / time;
}
}
private void ResolveTileCollision(IForwardMovablePhysicalEntity physicalEntity)
{
float timeStep = 0.01f; // can and should be small, because collision occurs in the next smallest time step
if (physicalEntity.ElasticCollision)
{
BounceFromTile(physicalEntity, timeStep);
}
else if (physicalEntity.InelasticCollision)
{
SlideAroundTile(physicalEntity, timeStep);
}
else
{
physicalEntity.ForwardSpeed = 0;
}
}
public void TestRotate(float rotationSpeed)
{
float startingDirection = 0;
var movableMock = new Mock <IForwardMovablePhysicalEntity>();
/*movableMock.Setup(x => x.Position).Returns(startingPosition);
* movableMock.Setup(x => x.ForwardSpeed).Returns(speed);
* movableMock.Setup(x => x.Direction).Returns(direction);*/
movableMock.SetupAllProperties();
movableMock.Object.Direction = startingDirection;
movableMock.Object.RotationSpeed = rotationSpeed;
IForwardMovablePhysicalEntity movable = movableMock.Object;
m_movementPhysics.Move(movable);
Assert.Equal(movable.Direction, startingDirection + rotationSpeed);
}
private void CollidesWithLine(IForwardMovablePhysicalEntity target, float normAngle)
{
if (target.ElasticCollision)
{
if (Math.Abs(MathHelper.WrapAngle(normAngle - target.Direction)) <= MathHelper.Pi / 2)
{
target.Direction = MathHelper.WrapAngle(2 * normAngle - MathHelper.Pi - target.Direction);
}
else
{
target.Direction = MathHelper.WrapAngle(2 * normAngle - target.Direction);
}
}
else if (target.InelasticCollision)
{
float cosWrtDirection0 = (float)Math.Sin(normAngle - target.Direction);
target.ForwardSpeed *= cosWrtDirection0;
target.Direction = normAngle;
}
else
{
target.ForwardSpeed = 0;
}
}
private void FindTileFreeDirection(IForwardMovablePhysicalEntity physicalEntity, float timeLeft)
{
if (physicalEntity.ElasticCollision)
{
BounceFromTile(physicalEntity, timeLeft);
}
else if (physicalEntity.InelasticCollision)
{
SlideAroundTile(physicalEntity, timeLeft);
}
else
{
physicalEntity.ForwardSpeed = 0;
}
}
private void ResolveCollisionOfTwo(IForwardMovablePhysicalEntity pe0, IForwardMovablePhysicalEntity pe1)
{
if (pe0 == pe1)
{
Debug.Assert(false);
}
Vector2 diff = (pe0.Position - pe1.Position);
Vector2 orthogonal = new Vector2(-diff.Y, diff.X);
Vector2 normalOrthogonal = Vector2.Normalize(orthogonal);
float orthogonalAngle0 = (float)Math.Atan2(normalOrthogonal.Y, normalOrthogonal.X);
float orthogonalAngle1 = (float)Math.Atan2(- normalOrthogonal.Y, - normalOrthogonal.X);
CollidesWithLine(pe0, orthogonalAngle0);
CollidesWithLine(pe1, orthogonalAngle1);
}
private void SlideAroundTile(IForwardMovablePhysicalEntity physicalEntity, float time)
{
Vector2 freePosition = physicalEntity.Position;
float directionRads = physicalEntity.Direction;
float speed = time*physicalEntity.ForwardSpeed;
float xSpeed = (float)Math.Sin(directionRads) * speed;
float ySpeed = (float)Math.Cos(directionRads) * speed;
// position before move
// try to move orthogonally left/right and up/down
TileFreePositionBinarySearch(physicalEntity, xSpeed, X_DIRECTION);
Vector2 xPosition = new Vector2(physicalEntity.Position.X, physicalEntity.Position.Y);
physicalEntity.Position = freePosition;
// try to move orthogonally up/down and left/right; reset position first
TileFreePositionBinarySearch(physicalEntity, ySpeed, Y_DIRECTION);
Vector2 yPosition = new Vector2(physicalEntity.Position.X, physicalEntity.Position.Y);
physicalEntity.Position = freePosition;
float distanceX = Vector2.Distance(freePosition, xPosition);
float distanceY = Vector2.Distance(freePosition, yPosition);
if (Math.Abs(distanceX) < NEGLIGIBLE_DISTANCE &&
Math.Abs(distanceY) < NEGLIGIBLE_DISTANCE)
{
physicalEntity.ForwardSpeed = 0;
return;
}
// farther position is chosen
if (distanceX > distanceY)
{
Vector2 diff = xPosition - freePosition;
physicalEntity.Direction = - (float)Math.Atan2(diff.X, diff.Y);
physicalEntity.ForwardSpeed = Math.Abs(xSpeed) / time;
}
else
{
Vector2 diff = yPosition - freePosition;
physicalEntity.Direction = (float)Math.Atan2(diff.X, diff.Y);
physicalEntity.ForwardSpeed = Math.Abs(ySpeed) / time;
}
}
private void BounceFromTile(IForwardMovablePhysicalEntity physicalEntity, float speed)
{
Vector2 originalPosition = physicalEntity.Position;
float originalDirection = physicalEntity.Direction;
float maxDistance = 0;
float bestDirection = originalDirection;
// candidate directions to move
var candidateDirections = new List<float>
{
MathHelper.WrapAngle(2f*MathHelper.Pi - physicalEntity.Direction),
MathHelper.WrapAngle(3f*MathHelper.Pi - physicalEntity.Direction)
};
// search for direction of longest move
foreach (float newDirection in candidateDirections)
{
TileFreePositionBinarySearch(physicalEntity, speed, newDirection);
float distance = Vector2.Distance(originalPosition, physicalEntity.Position);
if (maxDistance < distance)
{
maxDistance = distance;
bestDirection = newDirection;
}
physicalEntity.Position = originalPosition;
}
physicalEntity.Direction = bestDirection;
}
public void Move(IForwardMovablePhysicalEntity movable)
{
Rotate(movable);
Shift(movable);
}
/// <summary>
/// Search for position close to obstacle in given direction. Must be on stable position when starting search.
/// </summary>
/// <param name="physicalEntity"></param>
/// <param name="initialSpeed"></param>
/// <param name="direction"></param>
private void TileFreePositionBinarySearch(IForwardMovablePhysicalEntity physicalEntity, float initialSpeed, float direction)
{
float speed = initialSpeed;
Vector2 lastNotColliding = physicalEntity.Position;
bool goForward = true;
for (int i = 0; i < BINARY_SEARCH_ITERATIONS; i++)
{
if (goForward)
{
m_movementPhysics.Shift(physicalEntity, speed, direction);
}
else
{
m_movementPhysics.Shift(physicalEntity, -speed, direction);
}
bool colliding = m_collisionChecker.CollidesWithTile(physicalEntity);
if (!colliding)
{
lastNotColliding = physicalEntity.Position;
}
speed = speed / 2;
goForward = !colliding;
}
physicalEntity.Position = lastNotColliding;
}
public void RevertMoveKeepRotation(IForwardMovablePhysicalEntity movable)
{
Shift(movable, -movable.ForwardSpeed);
}
public void Shift(IForwardMovablePhysicalEntity movable, float speed)
{
movable.Position = Utils.Move(movable.Position, movable.Direction, speed);
}
public void Shift(IForwardMovablePhysicalEntity movable, float speed, float directionInRads)
{
movable.Position = Utils.Move(movable.Position, directionInRads, speed);
}
private static void Rotate(IForwardMovablePhysicalEntity movable, float rotationSpeed)
{
movable.Direction = MathHelper.WrapAngle(movable.Direction + rotationSpeed);
}
private void CollidesWithLine(IForwardMovablePhysicalEntity target, float normAngle)
{
if (target.ElasticCollision)
{
if (Math.Abs(MathHelper.WrapAngle(normAngle - target.Direction)) <= MathHelper.Pi / 2)
{
target.Direction = MathHelper.WrapAngle(2 * normAngle - MathHelper.Pi - target.Direction);
}
else
{
target.Direction = MathHelper.WrapAngle(2 * normAngle - target.Direction);
}
}
else if (target.InelasticCollision)
{
float cosWrtDirection0 = (float)Math.Sin(normAngle - target.Direction);
target.ForwardSpeed *= cosWrtDirection0;
target.Direction = normAngle;
}
else
{
target.ForwardSpeed = 0;
}
}
