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