Help with Collision Resolution?
- by Milo
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);
//velocity = Vector2D.add(velocity, Vector2D.scalarMultiply(acceleration, 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.postRotate(JMath.radToDeg(getAngle()));
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.postRotate(JMath.radToDeg(-getAngle()));
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)
{
//add linear force
forces.x += worldForce.x;
forces.y += worldForce.y;
//add associated torque
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 float wheelRadius;
private Vector2D position = new Vector2D();
public Wheel(Vector2D position, float radius)
{
this.position = position;
setSteeringAngle(0);
wheelSpeed = 0;
wheelRadius = radius;
wheelInertia = (radius * radius) * 1.1f;
}
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]);
}
public void addTransmissionTorque(float newValue)
{
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(
Vector2D.negative(forwardVec), wheelSpeed), wheelRadius);
//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
wheelTorque += forwardMag * wheelRadius;
//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)
{
wheels[0].addTransmissionTorque(throttle * torque);
wheels[1].addTransmissionTorque(throttle * torque);
}
wheels[2].addTransmissionTorque(throttle * torque);
wheels[3].addTransmissionTorque(throttle * torque);
}
public void setBrakes(float brakes)
{
float brakeTorque = 15.0f;
//apply brake torque opposing wheel vel
for (Wheel wheel : wheels)
{
float wheelVel = wheel.getWheelSpeed();
wheel.addTransmissionTorque(-wheelVel * brakeTorque * brakes);
}
}
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)
wheel.addTransmissionTorque(-wheelVel * 0.11f);
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)).
add(delta.multiply(-1.0f)));
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);
corner[1] = center.add(X).subtract(Y);
corner[2] = center.add(X).add(Y);
corner[3] = center.subtract(X).add(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 centroid = (corner[0].add(corner[1]).add(corner[2]).add(corner[3])).divide(4.0f);
Vector2D translation = center.subtract(centroid);
for (int c = 0; c < 4; ++c)
{
corner[c] = corner[c].add(translation);
}
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?
http://www.google.ca/url?sa=t&rct=j&q=&esrc=s&source=web&cd=2&ved=0CC8QFjAB&url=http%3A%2F%2Fcoitweb.uncc.edu%2F~tbarnes2%2FGameDesignFall05%2FSlides%2FCh4.2-CollDet.ppt&ei=x4ucULy5M6-N0QGRy4D4Cg&usg=AFQjCNG7FVDXWRdLv8_-T5qnFyYld53cTQ&cad=rja
