573 lines
17 KiB
C++
573 lines
17 KiB
C++
#include <cstring>
|
|
#include<coll2d.h>
|
|
|
|
#include <iostream>
|
|
|
|
using namespace std;
|
|
|
|
namespace coll2d {
|
|
|
|
int check_collision (float timestep, Shape *shape_a, Shape *shape_b, CollisionInfo *info) {
|
|
check_cb check = NULL;
|
|
|
|
check = get_check (shape_a, shape_b);
|
|
|
|
if ( check == NULL ) {
|
|
return CHECK_ERROR_NOT_IMPLEMENTED;
|
|
}
|
|
|
|
return check (timestep, shape_a, shape_b, info);
|
|
};
|
|
|
|
check_cb get_check (Shape *shape_a, Shape *shape_b) {
|
|
if ( (dynamic_cast<Polygon*> (shape_a) != NULL)
|
|
&& (dynamic_cast<Sphere*> (shape_b) != NULL) ) {
|
|
return check_collision_polygon_sphere;
|
|
}
|
|
else if ( (dynamic_cast<Polygon*> (shape_b) != NULL)
|
|
&& (dynamic_cast<Sphere*> (shape_a) != NULL) ) {
|
|
return check_collision_polygon_sphere;
|
|
} else if ( (dynamic_cast<Sphere*> (shape_b) != NULL)
|
|
&& (dynamic_cast<Sphere*> (shape_a) != NULL) ) {
|
|
return check_collision_sphere_sphere;
|
|
}
|
|
|
|
return NULL;
|
|
}
|
|
|
|
int check_collision_rel (float timestep, Shape *shape_a, Shape *shape_b, vector3d *velocity_b, CollisionInfo *info) {
|
|
check_cb check = NULL;
|
|
|
|
shape_b->setVelocity (*velocity_b);
|
|
check = get_check (shape_a, shape_b);
|
|
|
|
if ( check == NULL ) {
|
|
return CHECK_ERROR_NOT_IMPLEMENTED;
|
|
}
|
|
|
|
return check (timestep, shape_a, shape_b, info);
|
|
};
|
|
|
|
/** \brief calculates the time for a moving point to touch a plane
|
|
*
|
|
* \param normal The normal of the plane
|
|
* \param plane_point a point on the plane
|
|
* \param point the moving point
|
|
* \param velocity the velocity of the point
|
|
*
|
|
* \returns If the return value is negative, the point is moving away from the
|
|
* plane or is below the plane and thus does not touch it. Otherwise
|
|
* point + (return value) * velocity
|
|
* is on the plane.
|
|
*/
|
|
float calculate_contact_plane_point (vector3d &normal, vector3d &plane_point, vector3d &point, vector3d &velocity) {
|
|
vector3d temp_vector (point);
|
|
temp_vector -= plane_point;
|
|
|
|
// If the following is < 0 then the point is "below" the plane
|
|
if (normal * temp_vector < 0) {
|
|
// If the following is < 0 then we move even deeper
|
|
if ( normal * velocity < 0 )
|
|
return -999;
|
|
// Or towards the upper side of the plane.
|
|
else
|
|
return -111;
|
|
}
|
|
|
|
int i,imax = -1;
|
|
float vmax = 0.;
|
|
for (i = 0; i < 3; i++) {
|
|
if ( normal[i] * velocity[i] < vmax) {
|
|
vmax = normal[i] * velocity[i];
|
|
imax = i;
|
|
}
|
|
}
|
|
|
|
if (imax == -1) {
|
|
return -1.;
|
|
}
|
|
|
|
return (normal[imax] * plane_point[imax] - normal[imax] * point[imax]) / vmax;
|
|
}
|
|
|
|
// #define VERBOSE_PHASE
|
|
|
|
/** \brief Checks whether a sphere penetrates one of the polygones edges
|
|
*
|
|
* This check only finds intersections of the sphere with one of the sides in
|
|
* other words it will not find intersections with the vertices. As for convex
|
|
* polygons a sphere will either touch one of the edges or one of the
|
|
* vertices.
|
|
*
|
|
* First thing we do is calculate the normals pointing out of the polygon.
|
|
* Then for each edge e of the polygon (is being done by the function
|
|
* calculate_contatc_plane_point (...):
|
|
* - Calculate the point that would be the first to touch edge e if a contact
|
|
* occurs
|
|
* - Calculate the time it would take for this point to touch the edge with
|
|
* the relative velocity of the sphere
|
|
*
|
|
* Next we calculatefor each edge the point on the sphere which would be the first to touch
|
|
* the polygon, if it was moving towards the current edge.
|
|
*/
|
|
int check_collision_polygon_sphere_sides (Shape *shape_a, Shape *shape_b, CollisionInfo *info) {
|
|
Polygon* polygon = dynamic_cast<Polygon*> (shape_a);
|
|
Sphere* sphere = dynamic_cast<Sphere*> (shape_b);
|
|
|
|
if ( polygon == NULL && sphere == NULL) {
|
|
polygon = dynamic_cast<Polygon*> (shape_b);
|
|
sphere = dynamic_cast<Sphere*> (shape_a);
|
|
}
|
|
|
|
if (!polygon || !sphere) {
|
|
return CHECK_ERROR_INVALID_TYPES;
|
|
}
|
|
|
|
vector3d velocity_sphere = sphere->getVelocity();
|
|
|
|
vector3d temp_vector (velocity_sphere);
|
|
if (temp_vector.length2() == 0.)
|
|
return 0;
|
|
|
|
// Calculate the normals pointing OUT of the polygon
|
|
float *t = NULL, t_min = 10000.;
|
|
unsigned int i,j,vertices,count = 0;
|
|
vertices = polygon->getVerticeCount();
|
|
vector3d *normals = NULL;
|
|
vector3d side (0., 0., 0.);
|
|
vector3d up (0., 1., 0.);
|
|
/*
|
|
if (polygon->getPosition().length() > 0.) {
|
|
for (i = 0; i < vertices; i++) {
|
|
polygon->getVertice(i) += polygon->getPosition();
|
|
}
|
|
}
|
|
*/
|
|
// normals contains all the normal vectors out of
|
|
// the polygon
|
|
normals = new vector3d[vertices];
|
|
// The array t contains the time it takes until the sphere
|
|
// would touch the plane with the given velocity
|
|
t = new float[vertices];
|
|
|
|
for (i = 0; i < vertices; i++) {
|
|
j = (i + 1) % vertices;
|
|
side = polygon->getVertice(j);
|
|
side -= polygon->getVertice(i);
|
|
normals[i] = cross_product (side, up);
|
|
normals[i] /= normals[i].length ();
|
|
|
|
// temp_vector is the point on the sphere that would touch the plane first
|
|
// if it was directly moving towards it
|
|
temp_vector = normals[i];
|
|
temp_vector *= - sphere->getRadius();
|
|
temp_vector += sphere->getPosition();
|
|
|
|
t[i] = calculate_contact_plane_point (normals[i], polygon->getVertice(i), temp_vector, velocity_sphere);
|
|
|
|
#ifdef VERBOSE_PHASE
|
|
cout << "= next =" << endl;
|
|
cout << "t[" << i << "] = " << t[i] << " normal = "; normals[i].print ();
|
|
cout << "point = "; temp_vector.print();
|
|
cout << "plane point = "; polygon->getVertice(i).print();
|
|
#endif
|
|
if (t[i] < 0)
|
|
normals[i].setValues (0., 0., 0.);
|
|
else {
|
|
if (t[i] < t_min)
|
|
t_min = t[i];
|
|
count ++;
|
|
}
|
|
}
|
|
|
|
if (count == 0) {
|
|
delete[] t;
|
|
delete[] normals;
|
|
return 0;
|
|
}
|
|
|
|
vector3d contact_point, cp_near;
|
|
|
|
/// \ToDo: Instead of checking all planes, remember which ones to check
|
|
for (i = 0; i < vertices; i++) {
|
|
if ( t[i] >= 0) {
|
|
// So there seems to occur an collision. Now we have to check,
|
|
// whether this is actually between the points that define the
|
|
// plane.
|
|
unsigned int min_distance_vertex_index = i, other_vertice = (i + 1) % vertices;
|
|
float min_distance;
|
|
j = (i + 1) % vertices;
|
|
// First we calculate the actual contact point:
|
|
temp_vector = normals[i];
|
|
temp_vector *= - sphere->getRadius();
|
|
temp_vector *= t[i];
|
|
temp_vector += sphere->getPosition();
|
|
|
|
contact_point = normals[i];
|
|
contact_point *= -sphere->getRadius();
|
|
contact_point += temp_vector;
|
|
|
|
// Now we calculate to which vertice this point is closer:
|
|
temp_vector = contact_point;
|
|
temp_vector -= polygon->getVertice(i);
|
|
min_distance = temp_vector.length2 ();
|
|
|
|
temp_vector = contact_point;
|
|
temp_vector -= polygon->getVertice(j);
|
|
|
|
if (min_distance > temp_vector.length2 ()) {
|
|
min_distance_vertex_index = j;
|
|
other_vertice = i;
|
|
}
|
|
|
|
// Then we want to see, whether the contact point actually lies
|
|
// on the vector that goes from one vertice to the other. For
|
|
// this we calculate (cp - near) * (far - near). The value
|
|
// cannot be greater than 1. (otherwise we would have picked
|
|
// the other vector as near. If it is < 0 then it is outside
|
|
// of the polygon.
|
|
temp_vector = polygon->getVertice(other_vertice);
|
|
temp_vector -= polygon->getVertice(min_distance_vertex_index);
|
|
|
|
temp_vector /= temp_vector.length();
|
|
|
|
cp_near = contact_point;
|
|
cp_near -= polygon->getVertice(min_distance_vertex_index);
|
|
|
|
float val = cp_near * temp_vector;
|
|
if ( (val >= 0 && val <= 1) && (t[i] <= 1.)) {
|
|
info->time = t[i];
|
|
info->normal = normals[i];
|
|
|
|
info->point = sphere->getPosition();
|
|
info->point += sphere->getVelocity() * t[i];
|
|
info->point -= normals[i] * sphere->getRadius ();
|
|
|
|
delete[] t;
|
|
delete[] normals;
|
|
return 1;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
if ( (t_min <= 1.) && (t_min >= 0.)) {
|
|
return 1;
|
|
}
|
|
*/
|
|
|
|
delete[] t;
|
|
delete[] normals;
|
|
|
|
if (t_min > 1.)
|
|
return 0;
|
|
|
|
return 0;
|
|
// return CHECK_ERROR_UNKNOWN;
|
|
};
|
|
|
|
int check_collision_polygon_sphere_vertices (Shape *shape_a, Shape *shape_b, CollisionInfo *info) {
|
|
Polygon* polygon = dynamic_cast<Polygon*> (shape_a);
|
|
Sphere* sphere = dynamic_cast<Sphere*> (shape_b);
|
|
|
|
if ( polygon == NULL && sphere == NULL) {
|
|
polygon = dynamic_cast<Polygon*> (shape_b);
|
|
sphere = dynamic_cast<Sphere*> (shape_a);
|
|
}
|
|
|
|
if (!polygon || !sphere) {
|
|
return CHECK_ERROR_INVALID_TYPES;
|
|
}
|
|
|
|
// Transform global velocities to relative velocities:
|
|
/*
|
|
sphere->setVelocity (sphere->getVelocity() - polygon->getVelocity());
|
|
*/
|
|
vector3d velocity_sphere = sphere->getVelocity();
|
|
|
|
vector3d temp_vector (velocity_sphere);
|
|
if (temp_vector.length2() == 0.) {
|
|
#ifdef VERBOSE_PHASE
|
|
cout << "sphere has no velocity!" << endl;
|
|
#endif
|
|
return 0;
|
|
}
|
|
|
|
vector3d contact_point, cp_near;
|
|
|
|
// Okay if we happen to be here, there still might be one of
|
|
// the corners colliding with the sphere.
|
|
float a, b, c, t1, t2, t_min = 10000;
|
|
unsigned int i, j, vertices;
|
|
vector3d position (sphere->getPosition());
|
|
vector3d velocity (sphere->getVelocity());
|
|
float radius = sphere->getRadius();
|
|
vertices = polygon->getVerticeCount();
|
|
|
|
for (i = 0; i < vertices; i++) {
|
|
vector3d vertice (polygon->getVertice(i));
|
|
a = b = c = 0;
|
|
|
|
// We must ensure that all happens in the x-z-plane (so y == 0.)
|
|
vertice[1] = 0.;
|
|
position[1] = 0.;
|
|
|
|
vector3d distance_sphere_vertice;
|
|
for (j = 0; j < 3; j++) {
|
|
distance_sphere_vertice[j] = position[j] - vertice[j];
|
|
}
|
|
#ifdef VERBOSE_PHASE
|
|
cout << " ===== " << endl;
|
|
cout << "vertice = ";
|
|
vertice.print ();
|
|
velocity.print ();
|
|
position.print ();
|
|
cout << "radius = " << radius << endl;
|
|
cout << "distance = " << distance_sphere_vertice.length () << endl;;
|
|
#endif
|
|
for (j = 0; j < 3; j++) {
|
|
a += velocity[j]*velocity[j];
|
|
b += 2 * velocity[j] * (position[j] - vertice[j]);
|
|
c += position[j] * position[j] - 2 * position[j] * vertice[j] + vertice[j] * vertice[j];
|
|
}
|
|
c -= radius * radius;
|
|
|
|
if (solve_quadratic (a, b, c, &t1, &t2)) {
|
|
// cout << "solve_quadratic = " << t1 << "\t" << t2 << endl;
|
|
float t_temp_min = t2;
|
|
if (t1 < t2)
|
|
t_temp_min = t1;
|
|
|
|
if (t_temp_min < t_min) {
|
|
if ((t_temp_min <= 1.) && (t_temp_min >= 0.)) {
|
|
t_min = t_temp_min;
|
|
temp_vector = position;
|
|
temp_vector -= vertice;
|
|
temp_vector /= temp_vector.length ();
|
|
#ifdef VERBOSE_PHASE
|
|
cout << "new closest point t = " << t_min << endl;
|
|
#endif
|
|
info->time = t_min;
|
|
info->normal = temp_vector;
|
|
info->point = vertice;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if ( (t_min <= 1.) && (t_min >= 0.)) {
|
|
return 1;
|
|
}
|
|
|
|
if (t_min > 1.)
|
|
return 0;
|
|
|
|
return CHECK_ERROR_UNKNOWN;
|
|
};
|
|
|
|
int check_collision_polygon_sphere (float timestep, Shape *shape_a, Shape *shape_b, CollisionInfo *info) {
|
|
/* If the first shape given is a sphere and the second one a shape, we have
|
|
* to remember it to set the right value to *info.
|
|
*/
|
|
bool swapped = false;
|
|
Polygon* polygon_cast_test = dynamic_cast<Polygon*> (shape_a);
|
|
Sphere* sphere_cast_test = dynamic_cast<Sphere*> (shape_b);
|
|
|
|
if ( polygon_cast_test == NULL && sphere_cast_test == NULL) {
|
|
polygon_cast_test = dynamic_cast<Polygon*> (shape_b);
|
|
sphere_cast_test = dynamic_cast<Sphere*> (shape_a);
|
|
|
|
swapped = true;
|
|
}
|
|
|
|
if (!polygon_cast_test || !sphere_cast_test) {
|
|
return CHECK_ERROR_INVALID_TYPES;
|
|
}
|
|
|
|
Polygon polygon_copy (*polygon_cast_test);
|
|
Sphere sphere_copy (*sphere_cast_test);
|
|
|
|
unsigned int vertices;
|
|
vertices = polygon_copy.getVerticeCount();
|
|
|
|
#ifdef VERBOSE_PHASE
|
|
cout << "======== New Polygon Sphere Test: Phase 1" << endl;
|
|
if (swapped)
|
|
cout << "Swapped!" << endl;
|
|
cout << "Polygon position: ";
|
|
polygon_copy.getPosition().print ();
|
|
cout << "Polygon velocity: ";
|
|
polygon_copy.getVelocity().print ();
|
|
cout << "Sphere position: ";
|
|
sphere_copy.getPosition().print ();
|
|
cout << "Sphere velocity: ";
|
|
sphere_copy.getVelocity().print ();
|
|
cout << "Timestep : " << timestep << endl;
|
|
|
|
cout << "Vertices before transformation = " << endl;
|
|
int i;
|
|
for (i = 0; i < vertices; i++) {
|
|
polygon_copy.getVertice(i).print();
|
|
}
|
|
#endif
|
|
|
|
// Here we translate the polygon and its velocity into the reference frame
|
|
// of the polygon.
|
|
sphere_copy.setPosition (sphere_copy.getPosition() - polygon_copy.getPosition());
|
|
sphere_copy.setPosition (sphere_copy.getPosition().rotate_y (-polygon_copy.getAngle()));
|
|
// Here scale the velocity so that our time horizon lies in [0., 1.]
|
|
sphere_copy.setVelocity ((sphere_copy.getVelocity() - polygon_copy.getVelocity() ) * timestep );
|
|
sphere_copy.setVelocity (sphere_copy.getVelocity().rotate_y (-polygon_copy.getAngle()));
|
|
|
|
#ifdef VERBOSE_PHASE
|
|
cout << "Vertices after transformation = " << endl;
|
|
for (i = 0; i < vertices; i++) {
|
|
polygon_copy.getVertice(i).print();
|
|
}
|
|
#endif
|
|
|
|
#ifdef VERBOSE_PHASE
|
|
cout << "After transformation:" << endl;
|
|
cout << "Polygon position: ";
|
|
polygon_copy.getPosition().print ();
|
|
cout << "Polygon velocity: ";
|
|
polygon_copy.getVelocity().print ();
|
|
cout << "Sphere position: ";
|
|
sphere_copy.getPosition().print ();
|
|
cout << "Sphere velocity: ";
|
|
sphere_copy.getVelocity().print ();
|
|
#endif
|
|
|
|
CollisionInfo sides_info;
|
|
CollisionInfo vertices_info;
|
|
int sides_result = 0, vertices_result = 0;
|
|
|
|
/* Tricky part: Since polygons are assumed to be convex and we calculated
|
|
* with both methods a collision, we take the sides result. Since they are
|
|
* convex the first event to happen is the collision with the side.
|
|
* Otherwise it would first touch the vertice and then the side, which is
|
|
* not possible. (\Todo true?)
|
|
*/
|
|
|
|
sides_result = check_collision_polygon_sphere_sides (&polygon_copy, &sphere_copy, &sides_info);
|
|
// We have to transform the time back to [0., timestep]
|
|
sides_info.time *= timestep;
|
|
#ifdef VERBOSE_PHASE
|
|
cout << "sides_result = " << sides_result << " t = " << sides_info.time << endl;
|
|
#endif
|
|
if (sides_result > 0) {
|
|
sides_info.point.rotate_y (polygon_copy.getAngle());
|
|
sides_info.normal.rotate_y (polygon_copy.getAngle());
|
|
sides_info.point += polygon_copy.getPosition();
|
|
memcpy (info, &sides_info, sizeof (CollisionInfo));
|
|
sides_info.time *= timestep;
|
|
if (swapped == true) {
|
|
info->reference_shape = 1;
|
|
} else {
|
|
info->reference_shape = 0;
|
|
}
|
|
return sides_result;
|
|
}
|
|
|
|
vertices_result = check_collision_polygon_sphere_vertices (&polygon_copy, &sphere_copy, &vertices_info);
|
|
// We have to transform the time back to [0., timestep]
|
|
vertices_info.time *= timestep;
|
|
#ifdef VERBOSE_PHASE
|
|
cout << "vertices_res = " << vertices_result << " t = " << vertices_info.time << endl;
|
|
cout << "vertices_point = ";
|
|
vertices_info.point.print();
|
|
#endif
|
|
if (vertices_result > 0) {
|
|
vertices_info.point.rotate_y (polygon_copy.getAngle());
|
|
vertices_info.normal.rotate_y (polygon_copy.getAngle());
|
|
vertices_info.point += polygon_copy.getPosition();
|
|
memcpy (info, &vertices_info, sizeof (CollisionInfo));
|
|
if (swapped == true) {
|
|
info->reference_shape = 1;
|
|
} else {
|
|
info->reference_shape = 0;
|
|
}
|
|
return vertices_result;
|
|
}
|
|
|
|
if ((sides_result == 0) && (vertices_result == 0))
|
|
return 0;
|
|
|
|
return CHECK_ERROR_UNKNOWN;
|
|
};
|
|
|
|
int check_collision_sphere_sphere (float timestep, Shape *shape_a, Shape *shape_b, CollisionInfo *info) {
|
|
/* If the first shape given is a sphere and the second one a shape, we have
|
|
* to remember it to set the right value to *info.
|
|
*/
|
|
Sphere* sphere_test_a = dynamic_cast<Sphere*> (shape_a);
|
|
Sphere* sphere_test_b = dynamic_cast<Sphere*> (shape_b);
|
|
|
|
if (!sphere_test_a || !sphere_test_b) {
|
|
return CHECK_ERROR_INVALID_TYPES;
|
|
}
|
|
|
|
Sphere sphere_a (*sphere_test_a);
|
|
Sphere sphere_b (*sphere_test_b);
|
|
|
|
// First we check whether there is actually a relative velocity towards each
|
|
// other:
|
|
vector3d rel_velocity = sphere_b.getVelocity ();
|
|
rel_velocity -= sphere_a.getVelocity ();
|
|
rel_velocity *= timestep;
|
|
if (rel_velocity.length2() == 0.)
|
|
return 0;
|
|
|
|
vector3d rel_position = sphere_b.getPosition ();
|
|
rel_position -= sphere_a.getPosition ();
|
|
|
|
// We need to ignore height differences
|
|
rel_position[1] = 0.;
|
|
rel_velocity[1] = 0.;
|
|
|
|
vector3d rel_position_norm = rel_position;
|
|
rel_position_norm.normalize ();
|
|
|
|
float velocity_projection = rel_position_norm * rel_velocity;
|
|
float distance = rel_position.length();
|
|
|
|
if (velocity_projection >= 0.)
|
|
return 0;
|
|
|
|
float t = (- distance + sphere_a.getRadius () + sphere_b.getRadius ()) / velocity_projection;
|
|
|
|
#ifdef VERBOSE_PHASE
|
|
cout << "==== New Sphere Sphere Test ====" << endl;
|
|
cout << "Relative Position = ";
|
|
rel_position.print ();
|
|
cout << "Relative Velocity = ";
|
|
rel_velocity.print ();
|
|
cout << "velocity_projection = " << velocity_projection << endl;
|
|
cout << "distance = " << distance << endl;
|
|
cout << "- distance + Ra + Rb = " << - distance + sphere_a.getRadius () + sphere_b.getRadius () << endl;
|
|
cout << "t = " << t << endl;
|
|
#endif
|
|
|
|
// if t < 0 this means we would have to move back in time
|
|
// to get to the point where the two spheres touched. In other words: they
|
|
// are overlapping, hence it is an invalid state!
|
|
if (t < 0)
|
|
return CHECK_ERROR_OVERLAP;
|
|
if (t > 1)
|
|
return 0;
|
|
|
|
info->point = sphere_a.getPosition() + rel_position_norm * t;
|
|
info->time = t * timestep;
|
|
|
|
if (sphere_a.getVelocity().length2() == 0.) {
|
|
info->normal = rel_position_norm;
|
|
info->reference_shape = 0;
|
|
} else {
|
|
info->normal = rel_position_norm * -1.;
|
|
info->reference_shape = 1;
|
|
}
|
|
return 1;
|
|
};
|
|
|
|
}
|