# Help with Collision Resolution?

I'm trying to learn about physics by trying to make a simplified GTA 2 clone.

My only problem is collision resolution. Everything else works great.

I have a rigid body class and from there cars and a wheel class:

``````class RigidBody extends Entity
{
//linear
private Vector2D velocity = new Vector2D();
private Vector2D forces = new Vector2D();
private OBB2D predictionRect = new OBB2D(new Vector2D(), 1.0f, 1.0f, 0.0f);
private float mass;
private Vector2D deltaVec = new Vector2D();
private Vector2D v = new Vector2D();

//angular
private float angularVelocity;
private float torque;
private float inertia;

//graphical
private Vector2D halfSize = new Vector2D();
private Bitmap image;

private Matrix mat = new Matrix();
private float[] Vector2Ds = new float[2];
private Vector2D tangent = new Vector2D();
private static Vector2D worldRelVec = new Vector2D();
private static Vector2D relWorldVec = new Vector2D();
private static Vector2D pointVelVec = new Vector2D();

public RigidBody()
{
//set these defaults so we don't get divide by zeros
mass = 1.0f;
inertia = 1.0f;
setLayer(LAYER_OBJECTS);
}

protected void rectChanged()
{
if(getWorld() != null)
{
getWorld().updateDynamic(this);
}
}

//intialize out parameters
public void initialize(Vector2D halfSize, float mass, Bitmap bitmap)
{
//store physical parameters
this.halfSize = halfSize;
this.mass = mass;
image = bitmap;
inertia = (1.0f / 20.0f) * (halfSize.x * halfSize.x) * (halfSize.y * halfSize.y) * mass;

RectF rect = new RectF();
float scalar = 10.0f;
rect.left = (int)-halfSize.x * scalar;
rect.top = (int)-halfSize.y * scalar;

rect.right = rect.left + (int)(halfSize.x * 2.0f * scalar);
rect.bottom = rect.top + (int)(halfSize.y * 2.0f * scalar);
setRect(rect);
predictionRect.set(rect);

}

public void setLocation(Vector2D position, float angle)
{
getRect().set(position, getWidth(), getHeight(), angle);
rectChanged();
}

public void setPredictionLocation(Vector2D position, float angle)
{
getPredictionRect().set(position, getWidth(), getHeight(), angle);
}

public void setPredictionCenter(Vector2D center)
{
getPredictionRect().moveTo(center);
}

public void setPredictionAngle(float angle)
{
predictionRect.setAngle(angle);
}

public Vector2D getPosition()
{
return getRect().getCenter();
}

public OBB2D getPredictionRect()
{
return predictionRect;
}

@Override
public void update(float timeStep)
{
doUpdate(false,timeStep);
}

public void doUpdate(boolean prediction, float timeStep)
{
//integrate physics
//linear
Vector2D acceleration = Vector2D.scalarDivide(forces, mass);
if(prediction)
{
Vector2D velocity = Vector2D.add(this.velocity, Vector2D.scalarMultiply(acceleration, timeStep));
Vector2D c = getRect().getCenter();
c = Vector2D.add(getRect().getCenter(), Vector2D.scalarMultiply(velocity , timeStep));
setPredictionCenter(c);
//forces = new Vector2D(0,0); //clear forces
}
else
{
velocity.x += (acceleration.x * timeStep);
velocity.y += (acceleration.y * timeStep);
Vector2D c = getRect().getCenter();
v.x = getRect().getCenter().getX() + (velocity.x * timeStep);
v.y = getRect().getCenter().getY() + (velocity.y * timeStep);
deltaVec.x = v.x - c.x;
deltaVec.y = v.y - c.y;
deltaVec.normalize();
setCenter(v.x, v.y);
forces.x = 0; //clear forces
forces.y = 0;
}

//angular
float angAcc = torque / inertia;
if(prediction)
{
float angularVelocity = this.angularVelocity + angAcc * timeStep;
setPredictionAngle(getAngle() +  angularVelocity * timeStep);
//torque = 0; //clear torque
}
else
{
angularVelocity += angAcc * timeStep;
setAngle(getAngle() +  angularVelocity * timeStep);
torque = 0; //clear torque
}

}

public void updatePrediction(float timeStep)
{
doUpdate(true, timeStep);
}

//take a relative Vector2D and make it a world Vector2D
public Vector2D relativeToWorld(Vector2D relative)
{
mat.reset();

Vector2Ds[0] = relative.x;
Vector2Ds[1] = relative.y;

mat.mapVectors(Vector2Ds);

relWorldVec.x = Vector2Ds[0];
relWorldVec.y = Vector2Ds[1];
return new Vector2D(Vector2Ds[0], Vector2Ds[1]);
}

//take a world Vector2D and make it a relative Vector2D
public Vector2D worldToRelative(Vector2D world)
{
mat.reset();

Vector2Ds[0] = world.x;
Vector2Ds[1] = world.y;

mat.mapVectors(Vector2Ds);

return new Vector2D(Vector2Ds[0], Vector2Ds[1]);
}

//velocity of a point on body
public Vector2D pointVelocity(Vector2D worldOffset)
{
tangent.x = -worldOffset.y;
tangent.y = worldOffset.x;
return Vector2D.add( Vector2D.scalarMultiply(tangent, angularVelocity) , velocity);
}

public void applyForce(Vector2D worldForce, Vector2D worldOffset)
{
forces.x += worldForce.x;
forces.y += worldForce.y;
torque += Vector2D.cross(worldOffset, worldForce);
}

@Override
public void draw( GraphicsContext c)
{
c.drawRotatedScaledBitmap(image, getPosition().x, getPosition().y,
getWidth(), getHeight(), getAngle());
}

public Vector2D getVelocity()
{
return velocity;
}

public void setVelocity(Vector2D velocity)
{
this.velocity = velocity;
}

public Vector2D getDeltaVec()
{
return deltaVec;
}

}

Vehicle

public class Wheel
{
private Vector2D forwardVec;
private Vector2D sideVec;
private float wheelTorque;
private float wheelSpeed;
private float wheelInertia;
private Vector2D position = new Vector2D();

{
this.position = position;
setSteeringAngle(0);
wheelSpeed = 0;
}

public void setSteeringAngle(float newAngle)
{
Matrix mat = new Matrix();

float []vecArray = new float[4];
//forward Vector
vecArray[0] = 0;
vecArray[1] = 1;
//side Vector
vecArray[2] = -1;
vecArray[3] = 0;

mat.postRotate(newAngle / (float)Math.PI * 180.0f);
mat.mapVectors(vecArray);

forwardVec = new Vector2D(vecArray[0], vecArray[1]);
sideVec = new Vector2D(vecArray[2], vecArray[3]);
}

{
wheelTorque += newValue;
}

public float getWheelSpeed()
{
return wheelSpeed;
}

public Vector2D getAnchorPoint()
{
return position;
}

public Vector2D calculateForce(Vector2D relativeGroundSpeed, float timeStep, boolean prediction)
{
//calculate speed of tire patch at ground
Vector2D patchSpeed = Vector2D.scalarMultiply(Vector2D.scalarMultiply(

//get velocity difference between ground and patch
Vector2D velDifference = Vector2D.add(relativeGroundSpeed , patchSpeed);

//project ground speed onto side axis
Float forwardMag = new Float(0.0f);
Vector2D sideVel = velDifference.project(sideVec);
Vector2D forwardVel = velDifference.project(forwardVec, forwardMag);

//calculate super fake friction forces
//calculate response force
Vector2D responseForce = Vector2D.scalarMultiply(Vector2D.negative(sideVel), 2.0f);
responseForce =  Vector2D.subtract(responseForce, forwardVel);

float topSpeed = 500.0f;

//calculate torque on wheel

//integrate total torque into wheel
wheelSpeed += wheelTorque / wheelInertia * timeStep;

//top speed limit (kind of a hack)
if(wheelSpeed > topSpeed)
{
wheelSpeed = topSpeed;
}

//clear our transmission torque accumulator
wheelTorque = 0;

//return force acting on body
return responseForce;
}

public void setTransmissionTorque(float newValue)
{
wheelTorque = newValue;
}

public float getTransmissionTourque()
{
return wheelTorque;
}

public void setWheelSpeed(float speed)
{
wheelSpeed = speed;
}
}

//our vehicle object
public class Vehicle extends RigidBody
{

private Wheel [] wheels = new Wheel[4];
private boolean throttled = false;

public void initialize(Vector2D halfSize, float mass, Bitmap bitmap)
{
//front wheels
wheels[0] = new Wheel(new Vector2D(halfSize.x, halfSize.y), 0.45f);
wheels[1] = new Wheel(new Vector2D(-halfSize.x, halfSize.y), 0.45f);

//rear wheels
wheels[2] = new Wheel(new Vector2D(halfSize.x, -halfSize.y), 0.75f);
wheels[3] = new Wheel(new Vector2D(-halfSize.x, -halfSize.y), 0.75f);

super.initialize(halfSize, mass, bitmap);
}

public void setSteering(float steering)
{
float steeringLock = 0.13f;

//apply steering angle to front wheels
wheels[0].setSteeringAngle(steering * steeringLock);
wheels[1].setSteeringAngle(steering * steeringLock);

}

public void setThrottle(float throttle, boolean allWheel)
{
float torque = 85.0f;
throttled = true;

//apply transmission torque to back wheels
if (allWheel)
{
}

}

public void setBrakes(float brakes)
{
float brakeTorque = 15.0f;

//apply brake torque opposing wheel vel
for (Wheel wheel : wheels)
{
float wheelVel = wheel.getWheelSpeed();
}
}

public void doUpdate(float timeStep, boolean prediction)
{
for (Wheel wheel : wheels)
{
float wheelVel = wheel.getWheelSpeed();

//apply negative force to naturally slow down car
if(!throttled && !prediction)

Vector2D worldWheelOffset = relativeToWorld(wheel.getAnchorPoint());
Vector2D worldGroundVel = pointVelocity(worldWheelOffset);
Vector2D relativeGroundSpeed = worldToRelative(worldGroundVel);
Vector2D relativeResponseForce = wheel.calculateForce(relativeGroundSpeed, timeStep,prediction);
Vector2D worldResponseForce = relativeToWorld(relativeResponseForce);

applyForce(worldResponseForce, worldWheelOffset);
}

//no throttling yet this frame
throttled = false;

if(prediction)
{
super.updatePrediction(timeStep);
}
else
{
super.update(timeStep);
}

}

@Override
public void update(float timeStep)
{
doUpdate(timeStep,false);
}

public void updatePrediction(float timeStep)
{
doUpdate(timeStep,true);
}

public void inverseThrottle()
{
float scalar = 0.2f;
for(Wheel wheel : wheels)
{
wheel.setTransmissionTorque(-wheel.getTransmissionTourque() * scalar);
wheel.setWheelSpeed(-wheel.getWheelSpeed() * 0.1f);
}
}
}
``````

And my big hack collision resolution:

``````private void update()
{

camera.setPosition((vehicle.getPosition().x * camera.getScale()) - ((getWidth() ) / 2.0f),
(vehicle.getPosition().y * camera.getScale()) - ((getHeight() ) / 2.0f));

//camera.move(input.getAnalogStick().getStickValueX() * 15.0f, input.getAnalogStick().getStickValueY() * 15.0f);
if(input.isPressed(ControlButton.BUTTON_GAS))
{
vehicle.setThrottle(1.0f, false);
}

if(input.isPressed(ControlButton.BUTTON_STEAL_CAR))
{
vehicle.setThrottle(-1.0f, false);
}

if(input.isPressed(ControlButton.BUTTON_BRAKE))
{
vehicle.setBrakes(1.0f);
}

vehicle.setSteering(input.getAnalogStick().getStickValueX());
//vehicle.update(16.6666666f / 1000.0f);

boolean colided = false;

vehicle.updatePrediction(16.66666f / 1000.0f);

List<Entity> buildings = world.queryStaticSolid(vehicle,vehicle.getPredictionRect());

if(buildings.size() > 0)
{
colided = true;
}

if(!colided)
{
vehicle.update(16.66f / 1000.0f);
}
else
{
Vector2D delta = vehicle.getDeltaVec();
vehicle.setVelocity(Vector2D.negative(vehicle.getVelocity().multiply(0.2f)).
vehicle.inverseThrottle();
}

}
``````

Here is OBB

``````public class OBB2D
{

// Corners of the box, where 0 is the lower left.
private  Vector2D corner[] = new Vector2D[4];

private Vector2D center = new Vector2D();
private Vector2D extents = new Vector2D();

private RectF boundingRect = new RectF();
private float angle;

//Two edges of the box extended away from corner[0].
private  Vector2D axis[] = new Vector2D[2];

private double origin[] = new double[2];

public OBB2D(Vector2D center, float w, float h, float angle)
{
set(center,w,h,angle);
}

public OBB2D(float left, float top, float width, float height)
{
set(new Vector2D(left + (width / 2), top + (height / 2)),width,height,0.0f);
}

public void set(Vector2D center,float w, float h,float angle)
{
Vector2D X = new Vector2D( (float)Math.cos(angle), (float)Math.sin(angle));
Vector2D Y = new Vector2D((float)-Math.sin(angle), (float)Math.cos(angle));

X = X.multiply( w / 2);
Y = Y.multiply( h / 2);

corner[0] = center.subtract(X).subtract(Y);

computeAxes();
extents.x = w / 2;
extents.y = h / 2;
computeDimensions(center,angle);
}

private void computeDimensions(Vector2D center,float angle)
{
this.center.x = center.x;
this.center.y = center.y;
this.angle = angle;

boundingRect.left = Math.min(Math.min(corner[0].x, corner[3].x), Math.min(corner[1].x, corner[2].x));
boundingRect.top = Math.min(Math.min(corner[0].y, corner[1].y),Math.min(corner[2].y, corner[3].y));
boundingRect.right = Math.max(Math.max(corner[1].x, corner[2].x), Math.max(corner[0].x, corner[3].x));
boundingRect.bottom = Math.max(Math.max(corner[2].y, corner[3].y),Math.max(corner[0].y, corner[1].y));
}

public void set(RectF rect)
{
set(new Vector2D(rect.centerX(),rect.centerY()),rect.width(),rect.height(),0.0f);
}

// Returns true if other overlaps one dimension of this.
private boolean overlaps1Way(OBB2D other)
{
for (int a = 0; a < axis.length; ++a) {

double t = other.corner[0].dot(axis[a]);

// Find the extent of box 2 on axis a
double tMin = t;
double tMax = t;

for (int c = 1; c < corner.length; ++c) {
t = other.corner[c].dot(axis[a]);

if (t < tMin) {
tMin = t;
} else if (t > tMax) {
tMax = t;
}
}

// We have to subtract off the origin

// See if [tMin, tMax] intersects [0, 1]
if ((tMin > 1 + origin[a]) || (tMax < origin[a])) {
// There was no intersection along this dimension;
// the boxes cannot possibly overlap.
return false;
}
}

// There was no dimension along which there is no intersection.
// Therefore the boxes overlap.
return true;
}

//Updates the axes after the corners move.  Assumes the
//corners actually form a rectangle.
private void computeAxes()
{
axis[0] = corner[1].subtract(corner[0]);
axis[1] = corner[3].subtract(corner[0]);

// Make the length of each axis 1/edge length so we know any
// dot product must be less than 1 to fall within the edge.

for (int a = 0; a < axis.length; ++a) {
axis[a] = axis[a].divide((axis[a].length() * axis[a].length()));
origin[a] = corner[0].dot(axis[a]);
}
}

public void moveTo(Vector2D center)
{

Vector2D translation = center.subtract(centroid);

for (int c = 0; c < 4; ++c)
{
}

computeAxes();
computeDimensions(center,angle);
}

// Returns true if the intersection of the boxes is non-empty.
public boolean overlaps(OBB2D other)
{
if(right() < other.left())
{
return false;
}

if(bottom() < other.top())
{
return false;
}

if(left() > other.right())
{
return false;
}

if(top() > other.bottom())
{
return false;
}

if(other.getAngle() == 0.0f && getAngle() == 0.0f)
{
return true;
}

return overlaps1Way(other) && other.overlaps1Way(this);
}

public Vector2D getCenter()
{
return center;
}

public float getWidth()
{
return extents.x * 2;
}

public float getHeight()
{
return extents.y * 2;
}

public void setAngle(float angle)
{
set(center,getWidth(),getHeight(),angle);
}

public float getAngle()
{
return angle;
}

public void setSize(float w,float h)
{
set(center,w,h,angle);
}

public float left()
{
return boundingRect.left;
}

public float right()
{
return boundingRect.right;
}

public float bottom()
{
return boundingRect.bottom;
}

public float top()
{
return boundingRect.top;
}

public RectF getBoundingRect()
{
return boundingRect;
}

public boolean overlaps(float left, float top, float right, float bottom)
{
if(right() < left)
{
return false;
}

if(bottom() < top)
{
return false;
}

if(left() > right)
{
return false;
}

if(top() > bottom)
{
return false;
}

return true;
}

};
``````

What I do is when I predict a hit on the car, I force it back. It does not work that well and seems like a bad idea.

What could I do to have more proper collision resolution. Such that if I hit a wall I will never get stuck in it and if I hit the side of a wall I can steer my way out of it.

Thanks

I found this nice ppt. It talks about pulling objects apart and calculating new velocities. How could I calc new velocities in my case?