FPSDemo/CollisionDetection.cpp at master · dalorin/FPSDemo · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
#include "CollisionDetection.h"

Vector3** init_edge_array()
{
	Vector3** edges = new Vector3*[3];
	edges[0] = new Vector3(1, 0, 0);
	edges[1] = new Vector3(0, 1, 0);
	edges[2] = new Vector3(0, 0, 1);

	return edges;
}

Vector3** CollisionDetection::m_edges = init_edge_array();

SATResult CollisionDetection::detectCollision(Object& obj1, Object& obj2)
{
	struct SATResult result;
	// Use bounding spheres to rapidly exclude possibility of collision.
    /*if ([Utils DistanceBetweenPoint:object1.position andPoint:object2.position]
        >= object1.boundingCircleRadius + object2.boundingCircleRadius)
    {
        result.intersect = NO;
    }
    GLfloat velocityMag = [object1.velocity getMagnitude] + [object2.velocity getMagnitude];
    if ([Utils DistanceBetweenPoint:object1.position andPoint:object2.position]
        >= object1.boundingCircleRadius + object2.boundingCircleRadius + velocityMag)
    {
        result.willIntersect = NO;
        return result;
    }*/
    result.intersect = true;
    result.willIntersect = true;
	//GLfloat minInterval = INFINITY;
	GLfloat minInterval = 100000.0f;
    Vector3 translationAxis;
    //printf("Running collision detection\n");
    // Find possible separation axes
    //NSMutableArray *edges = [NSMutableArray arrayWithArray:object1.edges];
    //[edges addObjectsFromArray:object2.edges];
    for (int i = 0; i < 3; i++)
    {

        //Vector3 axis = m_edges[i]->getNormal();
		Vector3 axis = *m_edges[i];
        axis.normalize();
        //printf("Normalized axis: %f,%f\n",axis.x,axis.y);

        GLfloat min1, max1, min2, max2;
        min1 = min2 = 10000.0f; max1 = max2 = -10000.0f;
		CollisionDetection::projectObjectOntoAxis(obj1, *(obj1.getCollider()), axis, min1, max1);
		CollisionDetection::projectObjectOntoAxis(obj2, *(obj2.getCollider()), axis, min2, max2);

		GLfloat interval = CollisionDetection::intervalDistance(min1, max1, min2, max2);
        //printf("Object1: %f-%f, Object2: %f-%f\n\n",min1,max1,min2,max2);

        if (interval >= 0)
        {
            result.intersect = false;
        }

		GLfloat velocityProjection = axis.dot(obj1.getVelocity() - obj2.getVelocity());

        if (velocityProjection < 0)
            min1 += velocityProjection;
        else
            max1 += velocityProjection;

		interval = CollisionDetection::intervalDistance(min1, max1, min2, max2);

        if (interval >= 0)
        {
            result.willIntersect = false;
        }

        //If the objects aren't intersecting and won't intersect, exit the loop
        if (!result.intersect && !result.willIntersect)
            break;

        //Calculate the minimum translation vector that we will use to push the objects apart
        interval = fabsf(interval);
        if (interval < minInterval)
        {
            minInterval = interval;
            translationAxis = axis;

            //Calculate translation axis direction
            Vector3 d = obj1.getPosition() - obj2.getPosition();
            if (translationAxis.dot(d) < 0)
                translationAxis = -translationAxis;
        }
    }

    result.minimumTranslationVector = translationAxis * minInterval;

    return result;
}

/**
 * projectObjectOntoAxis
 * Projects the given object onto the given axis. Returns object's minimum and maximum points on the axis.
 * Used in Seperating Axis Theorem ColDet.
 **/
void CollisionDetection::projectObjectOntoAxis(Object& obj, Box& collider, Vector3& axis, GLfloat& min, GLfloat& max)
{
	GLfloat dp;
	std::vector<Vector3*> &vertices = collider.getVertices(obj.getPosition());

	for (std::vector<Vector3*>::iterator vertex_it = vertices.begin();
		 vertex_it != vertices.end();
		 ++vertex_it)
    {
        dp = axis.dot(**vertex_it);
        if (dp < min)
            min = dp;
        if (dp > max)
            max = dp;

		delete *vertex_it;
    }
}

GLfloat CollisionDetection::intervalDistance(GLfloat min1, GLfloat max1, GLfloat min2, GLfloat max2)
{
    if (min1 < min2)
        return min2 - max1;
    else
        return min1 - max2;
}