//-----------------------------------------------------------------------------
// File: Response.cpp
//
// Desc: Implementation of the collision response
//
// Copyright (c) 2000 Telemachos of Peroxide
// www.peroxide.dk
//-----------------------------------------------------------------------------

#include "vectormath.h"

#define EPSILON 0.05f


//-----------------------------------------------------------------------------
// Name: GetPosition()
// Desc: Main collision detection function. This is what you call to get
//       a position.
//-----------------------------------------------------------------------------
D3DVECTOR Game::GetPosition(D3DVECTOR position, D3DVECTOR velocity) {
	
 D3DVECTOR scaledPosition, scaledVelocity;	 
 D3DVECTOR finalPosition;

 // the first thing we do is scale the player and his velocity to
 // ellipsoid space
 scaledPosition = position / ellipsoidRadius;
 scaledVelocity = velocity / ellipsoidRadius;	
	
 // call the recursive collision response function	
 finalPosition = collideWithWorld(scaledPosition, scaledVelocity);
 	
 // when the function returns the result is still in ellipsoid space, so
 // we have to scale it back to R3 before we return it 	
 finalPosition = finalPosition * ellipsoidRadius;
  
 return finalPosition; 	
	
}




//-----------------------------------------------------------------------------
// Name: collideWithWorld()
// Desc: Recursive part of the collision response. This function is the
//       one who actually calls the collision check on the meshes
//-----------------------------------------------------------------------------
D3DVECTOR Game::collideWithWorld(D3DVECTOR position, D3DVECTOR velocity) {
 
  D3DVECTOR pos;
 
 
  // do we need to worry ?
  if (lengthOfVector(velocity) < EPSILON)
    return position;
   	
 	
  D3DVECTOR destinationPoint = position + velocity;
 
  // reset the collision package we send to the mesh 
  collision->velocity = velocity;
  collision->sourcePoint = position;
  collision->foundCollision = FALSE;
  collision->stuck = FALSE;
  collision->nearestDistance = -1;	
  
  
  
  // get a pointer to your meshes in some way
  Mesh* pCollisionData = pData;
	
  // Check all meshes
  while(pCollisionData) {
   	pCollisionData->CheckCollision(collision);	
	pCollisionData = pCollisionData->pNext;
  } 
   	
  
  
  // check return value here, and possibly call recursively 	
	
  if (collision->foundCollision == FALSE)
   { 
     // if no collision move very close to the desired destination. 
     float l = lengthOfVector(velocity);
     D3DVECTOR V = velocity; 
     setLength(V, l-EPSILON);
     	
     // update the last safe position for future error recovery	
     collision->lastSafePosition = position;      
     
     // return the final position
     return position + V;
   }
  else { // There was a collision
   
    
    // If we are stuck, we just back up to last safe position
    if (collision->stuck) {
      return collision->lastSafePosition;
    
    
      
    // OK, first task is to move close to where we hit something :
    D3DVECTOR newSourcePoint;
                   
    // only update if we are not already very close
    if (collision->nearestDistance >= EPSILON) {
         
      D3DVECTOR V = velocity;
      setLength(V, collision->nearestDistance-EPSILON);
      newSourcePoint = collision->sourcePoint + V;
    }
    else
      newSourcePoint = collision->sourcePoint;

   

    // Now we must calculate the sliding plane
    D3DVECTOR slidePlaneOrigin = collision->nearestPolygonIntersectionPoint;
    D3DVECTOR slidePlaneNormal = newSourcePoint - collision->nearestPolygonIntersectionPoint;
  
      
    // We now project the destination point onto the sliding plane
    double l = intersectRayPlane(destinationPoint, slidePlaneNormal, 
                                 slidePlaneOrigin, slidePlaneNormal); 
    
  
    // We can now calculate a new destination point on the sliding plane
    D3DVECTOR newDestinationPoint;
    newDestinationPoint.x = destinationPoint.x + l * slidePlaneNormal.x;
    newDestinationPoint.y = destinationPoint.y + l * slidePlaneNormal.y;
    newDestinationPoint.z = destinationPoint.z + l * slidePlaneNormal.z;
   
    
    // Generate the slide vector, which will become our new velocity vector
    // for the next iteration
    D3DVECTOR newVelocityVector = newDestinationPoint - collision->nearestPolygonIntersectionPoint;
        
       
    // now we recursively call the function with the new position and velocity 
    collision->lastSafePosition = position;
    return collideWithWorld(newSourcePoint, newVelocityVector); 
    
  }
 
}