BOOL boundingBoxCollide(struct BoundingBox a, struct BoundingBox b)
{
struct Vector a1 = a.direction = vectorNormalize(a.direction);
struct Vector a2 = a.up = vectorNormalize(a.up);
struct Vector a3 = vectorCross(a1, a2);
struct Vector b1 = b.direction = vectorNormalize(b.direction);
struct Vector b2 = b.up = vectorNormalize(b.up);
struct Vector b3 = vectorCross(b1, b2);
if(boundingBoxSeparationTest(a1, a, b)>0) return FALSE;
if(boundingBoxSeparationTest(a2, a, b)>0) return FALSE;
if(boundingBoxSeparationTest(a3, a, b)>0) return FALSE;
if(boundingBoxSeparationTest(b1, a, b)>0) return FALSE;
if(boundingBoxSeparationTest(b2, a, b)>0) return FALSE;
if(boundingBoxSeparationTest(b3, a, b)>0) return FALSE;
if(boundingBoxSeparationTest(vectorCross(a1, b1), a, b)>0) return FALSE;
if(boundingBoxSeparationTest(vectorCross(a1, b2), a, b)>0) return FALSE;
if(boundingBoxSeparationTest(vectorCross(a1, b3), a, b)>0) return FALSE;
if(boundingBoxSeparationTest(vectorCross(a2, b1), a, b)>0) return FALSE;
if(boundingBoxSeparationTest(vectorCross(a2, b2), a, b)>0) return FALSE;
if(boundingBoxSeparationTest(vectorCross(a2, b3), a, b)>0) return FALSE;
if(boundingBoxSeparationTest(vectorCross(a3, b1), a, b)>0) return FALSE;
if(boundingBoxSeparationTest(vectorCross(a3, b2), a, b)>0) return FALSE;
if(boundingBoxSeparationTest(vectorCross(a3, b3), a, b)>0) return FALSE;
return TRUE;
}
void chaser_update (Dynarr entities)
{
Entity
active;
int
i = 0,
j = 0;
Chaser
chaser;
struct target_info
* target;
VECTOR3
direction,
distance;
while ((active = *(Entity *)dynarr_at (entities, i++)))
{
chaser = component_getData (entity_getAs (active, "chaser"));
direction = vectorCreate (0, 0, 0);
j = 0;
while ((target = *(struct target_info **)dynarr_at (chaser->targets, j++)))
{
distance = position_distanceBetween (active, target->target);
distance = vectorNormalize (&distance);
distance = vectorMultiplyByScalar (&distance, target->weight);
direction = vectorAdd (&direction, &distance);
}
direction = vectorNormalize (&direction);
// direction is NaN; can't use
if (direction.x != direction.x || direction.z != direction.z)
continue;
position_face (active, &direction);
walking_start (active);
}
}
struct BoundingBox boundingBoxInterpolate(const struct BoundingBox *a, const struct BoundingBox *b)
{
struct BoundingBox bb;
bb.position = vectorTimes( vectorAdd(a->position, b->position), 0.5 );
bb.direction = vectorNormalize(vectorAdd(a->direction, b->direction));
bb.up = vectorNormalize(vectorAdd(a->up, b->up));
bb.halfSize = vectorTimes( vectorAdd(a->halfSize, b->halfSize), 0.5 );
return bb;
}
static void position_updateAxesFromOrientation (POSITION pdata)
{
QUAT
r;
if (pdata == NULL || pdata->dirty == false)
return;
r = pdata->orientation;
/* FIXME?: wait, why are the side and front vectors transposed here? I
* guess it doesn't really matter, but...
* - xph 2011-03-10
*/
// these values constitute a 3x3 rotation matrix. opengl offsets are commented on each value. see the camera component for how these are used to create the cameraview matrix.
pdata->view.side.x = // [0]
1 -
2 * r.y * r.y -
2 * r.z * r.z;
pdata->view.side.y = // [4]
2 * r.x * r.y -
2 * r.w * r.z;
pdata->view.side.z = // [8]
2 * r.x * r.z +
2 * r.w * r.y;
pdata->view.up.x = // [1]
2 * r.x * r.y +
2 * r.w * r.z;
pdata->view.up.y = // [5]
1 -
2 * r.x * r.x -
2 * r.z * r.z;
pdata->view.up.z = // [9]
2 * r.y * r.z -
2 * r.w * r.x;
pdata->view.front.x = // [2]
2 * r.x * r.z -
2 * r.w * r.y;
pdata->view.front.y = // [6]
2 * r.y * r.z +
2 * r.w * r.x;
pdata->view.front.z = // [10]
1 -
2 * r.x * r.x -
2 * r.y * r.y;
pdata->move.front = vectorCross (&pdata->view.side, &pdata->move.up);
pdata->move.front = vectorNormalize (&pdata->move.front);
pdata->move.side = vectorCross (&pdata->move.up, &pdata->view.front);
pdata->move.side = vectorNormalize (&pdata->move.side);
pdata->move.up = vectorCreate (0, 1, 0);
pdata->orientation = quat_normalize (&pdata->orientation);
pdata->dirty = false;
}
double vectorPointPlaneDistance(struct Vector point1, struct Vector point2, struct Vector normal)
{
normal = vectorNormalize(normal);
double d1 = vectorDot(normal, point1);
double d2 = vectorDot(normal, point2);
return fabs(d1-d2);
}
void Mesh::createVertexnormals(void) {
int i, j, ii;
bool connect;
float normal[3];
for (i = 0; i < vertexcount; i++) {
bool found = false;
vectorSet(normal, 0, 0, 0);
for (j = 0; j < polygoncount; j++) {
connect = false;
class Polygon *polygon = &polygons[j];
for (ii = 0; ii < polygon->vertexcount; ii++) {
if (polygons[j].vertices[ii] == &(vertices[i])) {
connect = true;
}
}
if (connect) {
vectorAdd(normal, polygon->planenormal);
found = true;
}
}
if (found) {
vectorNormalize(vertices[i].normal, normal);
}
}
for (j = 0; j < polygoncount; j++) {
class Polygon *polygon = &polygons[j];
if (!polygon->realsmooth)
polygon->smooth = true;
}
}
bool line_polyHit (const LINE3 * const line, int points, const VECTOR3 * const poly, float * tAtIntersection)
{
VECTOR3
u, v,
plane,
hit;
LINE3
transLine;
float
t;
u = vectorSubtract (&poly[1], &poly[0]);
v = vectorSubtract (&poly[2], &poly[0]);
plane = vectorCross (&u, &v);
plane = vectorNormalize (&plane);
// see note in the line_triHit function
transLine.origin = vectorSubtract (&line->origin, &poly[0]);
transLine.dir = line->dir;
if (!line_planeHit (&transLine, &plane, &t))
return false;
hit = vectorCreate
(
line->origin.x + line->dir.x * t,
line->origin.y + line->dir.y * t,
line->origin.z + line->dir.z * t
);
if (pointInPoly (&hit, points, poly))
{
if (tAtIntersection)
*tAtIntersection = t;
return true;
}
return false;
}
template <typename T> float MagneticSensorLsm303::heading(vector<T> from)
{
vector<int32_t> temp_m = {magnetometer.x, magnetometer.y, magnetometer.z};
///< subtract offset (average of min and max) from magnetometer readings
temp_m.x -= ((int32_t)magnetometer_min.x + magnetometer_max.x) / 2;
temp_m.y -= ((int32_t)magnetometer_min.y + magnetometer_max.y) / 2;
temp_m.z -= ((int32_t)magnetometer_min.z + magnetometer_max.z) / 2;
///< compute E and N
vector<float> E;
vector<float> N;
vectorCross(&temp_m, &accelerometer, &E);
vectorNormalize(&E);
vectorCross(&accelerometer, &E, &N);
vectorNormalize(&N);
///< compute heading
float heading = atan2(vectorDot(&E, &from), vectorDot(&N, &from)) * 180 / M_PI;
if (heading < 0) heading += 360;
return heading;
}
_C3DTQuaternion quaternionFromAxisAngle(C3DTVector v) {
v = vectorNormalize(v);
_C3DTQuaternion q;
float sinHalfAngle = sinf(v.cartesian.w / 2.0f);
q.cartesian.x = v.cartesian.x * sinHalfAngle;
q.cartesian.y = v.cartesian.y * sinHalfAngle;
q.cartesian.z = v.cartesian.z * sinHalfAngle;
q.cartesian.w = cosf(v.cartesian.w / 2.0f);
return q;
}
// Compute p1,p2,p3 face normal into pOut
point Actor::computeFaceNormal (point *p1, point *p2, point *p3, point *pOut)
{
// Uses p2 as a new origin for p1,p3
point a;
vectorOffset(p3, p2, &a);
point b;
vectorOffset(p1, p2, &b);
// Compute the cross product a X b to get the face normal
point pn;
vectorGetNormal(&a, &b, &pn);
// Return a normalized vector
vectorNormalize(&pn, pOut);
return *pOut;
}
Plane::Plane(GzCoord aVertexList[]){
GzCoord temp1, temp2;
for(int i = 0; i < 3; i++){
for(int j = 0; j < 3; j++){
vertexList[i][j] = aVertexList[i][j];
}
}
vectorConstruct(vertexList[0], vertexList[1], temp1);
vectorConstruct(vertexList[0], vertexList[2], temp2);
vectorCrossProduct(temp1, temp2, normal);
vectorNormalize(normal);
distance = vectorDotProduct(normal, vertexList[0]);
}
void Mesh::createPlanes(void) {
int i;
for (i = 0; i < polygoncount; i++) {
class Polygon *polygon = &this->polygons[i];
if (polygon->vertexcount >= 3) {
float v1[3], v2[3];
vectorSub(v1, polygon->vertices[1]->position,
polygon->vertices[0]->position);
vectorSub(v2, polygon->vertices[2]->position,
polygon->vertices[0]->position);
vectorCross(polygon->planenormal, v1, v2);
vectorNormalize(polygon->planenormal);
polygon->planedistance =
-vectorDot(polygon->vertices[0]->position, polygon->planenormal);
}
}
}
// Define the view frustum for culling
C3DTFrustum viewFrustum(const C3DTMatrix projection, const C3DTMatrix model)
{
C3DTMatrix clip;
C3DTFrustum r;
clip = matrixMultiply(projection, model);
// Right plane
r.planes[0].cartesian.x = clip.flts[3] - clip.flts[0];
r.planes[0].cartesian.y = clip.flts[7] - clip.flts[4];
r.planes[0].cartesian.z = clip.flts[11] - clip.flts[8];
r.planes[0].cartesian.w = clip.flts[15] - clip.flts[12];
r.planes[0] = vectorNormalize(r.planes[0]);
// Left plane
r.planes[1].cartesian.x = clip.flts[3] + clip.flts[0];
r.planes[1].cartesian.y = clip.flts[7] + clip.flts[4];
r.planes[1].cartesian.z = clip.flts[11] + clip.flts[8];
r.planes[1].cartesian.w = clip.flts[15] + clip.flts[12];
r.planes[1] = vectorNormalize(r.planes[1]);
// Bottom plane
r.planes[2].cartesian.x = clip.flts[3] + clip.flts[1];
r.planes[2].cartesian.y = clip.flts[7] + clip.flts[5];
r.planes[2].cartesian.z = clip.flts[11] + clip.flts[9];
r.planes[2].cartesian.w = clip.flts[15] + clip.flts[13];
r.planes[2] = vectorNormalize(r.planes[2]);
// Top plane
r.planes[3].cartesian.x = clip.flts[3] - clip.flts[1];
r.planes[3].cartesian.y = clip.flts[7] - clip.flts[5];
r.planes[3].cartesian.z = clip.flts[11] - clip.flts[9];
r.planes[3].cartesian.w = clip.flts[15] - clip.flts[13];
r.planes[3] = vectorNormalize(r.planes[3]);
// Far plane
r.planes[4].cartesian.x = clip.flts[3] - clip.flts[2];
r.planes[4].cartesian.y = clip.flts[7] - clip.flts[6];
r.planes[4].cartesian.z = clip.flts[11] - clip.flts[10];
r.planes[4].cartesian.w = clip.flts[15] - clip.flts[14];
r.planes[4] = vectorNormalize(r.planes[4]);
// Near plane
r.planes[5].cartesian.x = clip.flts[3] + clip.flts[2];
r.planes[5].cartesian.y = clip.flts[7] + clip.flts[6];
r.planes[5].cartesian.z = clip.flts[11] + clip.flts[10];
r.planes[5].cartesian.w = clip.flts[15] + clip.flts[14];
r.planes[5] = vectorNormalize(r.planes[5]);
return r;
}
void textureDrawLine (TEXTURE t, VECTOR3 start, VECTOR3 end)
{
VECTOR3
diff = vectorSubtract (&end, &start),
norm = vectorNormalize (&diff),
current = start;
/* i'm using i/max as a counter since due to floating point rounding error
* it's not feasible to use vector_cmp (¤t, &end), which is the only
* other way i can think of to check for when the line is actually done
* - xph 2011 08 27
*/
int
i = 0,
max = vectorMagnitude (&diff) + 1;
/* if start is out of bounds skip ahead until it isn't (if it isn't), and
* if it goes out of bounds again then it's not coming back. this is
* important if start is out of bounds but end isn't (or if they're both
* oob but they cross the image edge at some point); the start of the line
* won't be drawn but stopping after the first oob coordiante would be
* wrong
* (there's a better way to do this: if either value is oob, calculate the
* intersection of the line w/ the screen edges and jump to that point
* without needing to loop)
* - xph 2011 08 27
*/
while (textureOOB (t, current) && i < max)
{
current = vectorAdd (¤t, &norm);
i++;
}
while (i < max)
{
if (textureOOB (t, current))
break;
textureDrawPixel (t, current);
current = vectorAdd (¤t, &norm);
i++;
}
}
struct Vector vectorRotate(double angle, struct Vector axis, struct Vector v)
{
if(vectorLength(axis) < EPSILON) // axis is zero
{
return v;
}
double c=cos(angle);
double s=sin(angle);
struct Vector result;
axis = vectorNormalize(axis);
result.x = (axis.x*axis.x*(1-c)+c )*v.x +
(axis.x*axis.y*(1-c)-axis.z*s)*v.y +
(axis.x*axis.z*(1-c)+axis.y*s)*v.z;
result.y = (axis.y*axis.x*(1-c)+axis.z*s)*v.x +
(axis.y*axis.y*(1-c)+c )*v.y +
(axis.y*axis.z*(1-c)-axis.x*s)*v.z;
result.z = (axis.z*axis.x*(1-c)-axis.y*s)*v.x +
(axis.z*axis.y*(1-c)+axis.x*s)*v.y +
(axis.z*axis.z*(1-c)+c )*v.z;
return result;
}
// Normal of 2 vectors (0 centered)
inline C3DTVector vectorNormal(const C3DTVector a, const C3DTVector b)
{
return vectorNormalize(vectorCrossProduct(a, b));
}
// Returns the cos() of the angle between 2 vectors
inline float vectorAngleCos(const C3DTVector a, const C3DTVector b)
{
return vectorDotProduct(vectorNormalize(a), vectorNormalize(b));
}
// Normal of 2 vectors (centered on a 3rd point)
inline C3DTVector vectorNormalTri(const C3DTVector a, const C3DTVector b, const C3DTVector c)
{
return vectorNormalize(vectorCrossProductTri(a, b, c));
}
bool handleCollision(Contact *contact) {
Object *source = contact->object1;
Object *target = contact->object2;
float *normal = contact->normal;
float *contactpoint = contact->position;
float sourcevelocity[3], targetvelocity[3];
float sourcecontactpoint[3], targetcontactpoint[3];
vectorSub(sourcecontactpoint, contactpoint, source->position);
source->getVelocity(sourcevelocity, sourcecontactpoint);
if (target == NULL) {
vectorSet(targetcontactpoint, 0, 0, 0);
vectorSet(targetvelocity, 0, 0, 0);
} else {
vectorSub(targetcontactpoint, contactpoint, target->position);
target->getVelocity(targetvelocity, targetcontactpoint);
}
float deltavelocity[3];
vectorSub(deltavelocity, sourcevelocity, targetvelocity);
float dot = vectorDot(deltavelocity, normal);
// if (fabs(dot) < EPSILON) return false;
// if (dot > -1.0e-5 && dot < 1.0e-5) return false;
// if (dot >= 0) return false;
if (dot > -1.0e-5)
return false;
float invmass1;
invmass1 = source->invmass;
float invmass2;
if (target == NULL)
invmass2 = 0;
else
invmass2 = target->invmass;
float t1;
if (source->invmomentofinertia == 0) {
t1 = 0;
} else {
float v1[3];
vectorCross(v1, sourcecontactpoint, normal);
vectorScale(v1, source->invmomentofinertia);
float w1[3];
vectorCross(w1, v1, sourcecontactpoint);
t1 = vectorDot(normal, w1);
}
float t2;
if (target == NULL || target->invmomentofinertia == 0) {
t2 = 0;
} else {
float v1[3];
vectorCross(v1, targetcontactpoint, normal);
vectorScale(v1, target->invmomentofinertia);
float w1[3];
vectorCross(w1, v1, targetcontactpoint);
t2 = vectorDot(normal, w1);
}
float denominator = invmass1 + invmass2 + t1 + t2;
float e = 1.0 - COLLISIONFRICTION;
float impulsesize = (1 + e) * dot / denominator;
// printf("%f\n", impulsesize);
float impulse[3];
vectorScale(impulse, normal, impulsesize);
float friction[3];
vectorScale(friction, normal, vectorDot(deltavelocity, normal));
vectorAdd(friction, deltavelocity);
vectorNormalize(friction);
float frictionsize = 10 * KINETICFRICTION * dot / denominator;
float maxfrictionsize = 0.1 * vectorLength(deltavelocity);
if (frictionsize < -maxfrictionsize)
frictionsize = -maxfrictionsize;
vectorScale(friction, -frictionsize);
vectorAdd(impulse, friction);
if (target != NULL) {
target->addImpulse(impulse, targetcontactpoint);
target->calculateStateVariables();
}
float speed;
float speed2[3];
if (target != NULL && source != NULL) {
// float kvel[3];
// source->getVelocity(kvel);
float k = vectorLength(sourcevelocity) * 0.1;
// if (k > 1) k = 1;
speed = -impulsesize * target->invmass * k;
vectorScale(speed2, impulse, target->invmass * k);
/*float kvel[3];
source->getVelocity(kvel);
float k = 0;//vectorDot(speed2, kvel);
if (k < EPSILON) k = 0;
speed *= k;
vectorScale(speed2, k);
if (k > 0) */ target->hitForce(speed, speed2, source);
}
vectorScale(impulse, -1);
source->addImpulse(impulse, sourcecontactpoint);
source->calculateStateVariables();
// vectorScale(speed, source->invmass);
if (target != NULL && source != NULL) {
// float kvel[3];
// target->getVelocity(kvel);
float k = vectorLength(targetvelocity) * 0.1;
// if (k > 1) k = 1;
speed = -impulsesize * source->invmass * k;
vectorScale(speed2, impulse, source->invmass * k);
/*float kvel[3];
target->getVelocity(kvel);
float k = 0;//vectorDot(speed2, kvel);
if (k < EPSILON) k = 0;
speed *= k;
vectorScale(speed2, k);
if (k > 0) */ source->hitForce(speed, speed2, target);
}
return true;
}
bool checkSphereMeshCollision(float *sphereposition, float r, Mesh *mesh,
float *normal, float *contactpoint) {
float linenormal[3];
float pointnormal[3];
float maxdist = 0;
bool planecollision = false;
bool linecollision = false;
bool pointcollision = false;
int i, j;
for (i = 0; i < mesh->polygoncount; i++) {
class Polygon *polygon = &mesh->polygons[i];
float dist = distanceFromPlane(sphereposition, polygon->planenormal,
polygon->planedistance);
if (dist < r && dist > -r) {
bool directcollision = true;
for (j = 0; j < polygon->vertexcount; j++) {
float *p1 = polygon->vertices[j]->position;
float *p2 = polygon->vertices[(j + 1) % polygon->vertexcount]->position;
float *p3 = polygon->vertices[(j + 2) % polygon->vertexcount]->position;
float v1[3], v2[3];
vectorSub(v1, p2, p1);
// Collision for polygon surface
vectorSub(v2, p3, p2);
float t1[3];
vectorProject(t1, v2, v1);
float norm[3];
vectorSub(norm, v2, t1);
vectorNormalize(norm);
// Collision for polygon edges
float newpoint[3];
vectorSub(newpoint, sphereposition, p1);
float dist2 = vectorDot(newpoint, norm);
if (dist2 < 0) {
directcollision = false;
float projloc = vectorDot(newpoint, v1) / vectorDot(v1, v1);
if (projloc >= 0 && projloc <= 1) {
float proj[3];
vectorScale(proj, v1, projloc);
float projorth[3];
vectorSub(projorth, newpoint, proj);
float l2 = vectorDot(projorth, projorth);
if (l2 < r * r) {
vectorNormalize(linenormal, projorth);
if (dist < 0)
vectorScale(linenormal, -1);
linecollision = true;
}
}
}
// Collision for polygon vertices
float pointdiff[3];
vectorSub(pointdiff, sphereposition, p1);
float l3 = vectorDot(pointdiff, pointdiff);
if (l3 < r * r) {
vectorScale(pointnormal, pointdiff, 1.0 / sqrt(l3));
if (dist < 0)
vectorScale(pointnormal, -1);
pointcollision = true;
}
}
if (directcollision) {
if (dist > maxdist || !planecollision) {
vectorCopy(normal, polygon->planenormal);
maxdist = dist;
planecollision = true;
}
}
}
}
if (planecollision) {
vectorScale(contactpoint, normal, -r);
vectorAdd(contactpoint, sphereposition);
} else if (linecollision) {
vectorScale(contactpoint, linenormal, -r);
vectorAdd(contactpoint, sphereposition);
vectorCopy(normal, linenormal);
} else if (pointcollision) {
vectorScale(contactpoint, pointnormal, -r);
vectorAdd(contactpoint, sphereposition);
vectorCopy(normal, pointnormal);
} else {
return false;
}
return true;
}
bool handleLink(ObjectLink *link) {
if (!link->enabled)
return false;
Object *source = link->object1;
Object *target = link->object2;
float normal[3];
float contactpoint1[3], contactpoint2[3];
source->transformPoint(contactpoint1, link->point1);
target->transformPoint(contactpoint2, link->point2);
float diff[3];
vectorSub(diff, contactpoint1, contactpoint2);
vectorNormalize(normal, diff);
float strength = vectorDot(diff, diff);
if (strength < 1.0e-5)
return false;
float sourcevelocity[3], targetvelocity[3];
float sourcecontactpoint[3], targetcontactpoint[3];
vectorSub(sourcecontactpoint, contactpoint1, source->position);
source->getVelocity(sourcevelocity, sourcecontactpoint);
vectorSub(targetcontactpoint, contactpoint2, target->position);
target->getVelocity(targetvelocity, targetcontactpoint);
float deltavelocity[3];
vectorSub(deltavelocity, sourcevelocity, targetvelocity);
float dot = vectorDot(deltavelocity, normal);
// if (fabs(dot) < EPSILON) return false;
// if (dot > -1.0e-5 && dot < 1.0e-5) return false;
// if (dot >= 0) return false;
// if (dot > -1.0e-5) return false;
float invmass1 = source->invmass;
float invmass2 = target->invmass;
float t1;
if (source->invmomentofinertia == 0) {
t1 = 0;
} else {
float v1[3];
vectorCross(v1, sourcecontactpoint, normal);
vectorScale(v1, source->invmomentofinertia);
float w1[3];
vectorCross(w1, v1, sourcecontactpoint);
t1 = vectorDot(normal, w1);
}
float t2;
if (target->invmomentofinertia == 0) {
t2 = 0;
} else {
float v1[3];
vectorCross(v1, targetcontactpoint, normal);
vectorScale(v1, target->invmomentofinertia);
float w1[3];
vectorCross(w1, v1, targetcontactpoint);
t2 = vectorDot(normal, w1);
}
float denominator = invmass1 + invmass2 + t1 + t2;
float impulsesize = (dot + strength * 100) / denominator;
// printf("%f\n", impulsesize);
float impulse[3];
vectorScale(impulse, normal, impulsesize);
target->addImpulse(impulse, targetcontactpoint);
target->calculateStateVariables();
vectorScale(impulse, -1);
source->addImpulse(impulse, sourcecontactpoint);
source->calculateStateVariables();
return true;
}
void boundingBoxCollisionPoint(struct Vector* normal, struct Vector* point, struct BoundingBox ai, struct BoundingBox bi, struct BoundingBox af, struct BoundingBox bf)
{
int i;
int separartingAxesNum = 0;
int lastSeparartingAxis = 0;
struct BoundingBox a;
struct BoundingBox b;
double distance, minDistance;
i=1000;
while(separartingAxesNum != 1 && i-->0)
{
separartingAxesNum = 0;
a = boundingBoxInterpolate(&ai, &af);
b = boundingBoxInterpolate(&bi, &bf);
struct Vector a1 = a.direction = vectorNormalize(a.direction);
struct Vector a2 = a.up = vectorNormalize(a.up);
struct Vector a3 = vectorCross(a1, a2);
struct Vector b1 = b.direction = vectorNormalize(b.direction);
struct Vector b2 = b.up = vectorNormalize(b.up);
struct Vector b3 = vectorCross(b1, b2);
if(boundingBoxSeparationTest(a1, a, b)>0) {separartingAxesNum++; lastSeparartingAxis=0;}
if(boundingBoxSeparationTest(a2, a, b)>0) {separartingAxesNum++; lastSeparartingAxis=1;}
if(boundingBoxSeparationTest(a3, a, b)>0) {separartingAxesNum++; lastSeparartingAxis=2;}
if(boundingBoxSeparationTest(b1, a, b)>0) {separartingAxesNum++; lastSeparartingAxis=3;}
if(boundingBoxSeparationTest(b2, a, b)>0) {separartingAxesNum++; lastSeparartingAxis=4;}
if(boundingBoxSeparationTest(b3, a, b)>0) {separartingAxesNum++; lastSeparartingAxis=5;}
if(boundingBoxSeparationTest(vectorCross(a1, b1), a, b)>0) {separartingAxesNum++; lastSeparartingAxis=6;}
if(boundingBoxSeparationTest(vectorCross(a1, b2), a, b)>0) {separartingAxesNum++; lastSeparartingAxis=7;}
if(boundingBoxSeparationTest(vectorCross(a1, b3), a, b)>0) {separartingAxesNum++; lastSeparartingAxis=8;}
if(boundingBoxSeparationTest(vectorCross(a2, b1), a, b)>0) {separartingAxesNum++; lastSeparartingAxis=9;}
if(boundingBoxSeparationTest(vectorCross(a2, b2), a, b)>0) {separartingAxesNum++; lastSeparartingAxis=10;}
if(boundingBoxSeparationTest(vectorCross(a2, b3), a, b)>0) {separartingAxesNum++; lastSeparartingAxis=11;}
if(boundingBoxSeparationTest(vectorCross(a3, b1), a, b)>0) {separartingAxesNum++; lastSeparartingAxis=12;}
if(boundingBoxSeparationTest(vectorCross(a3, b2), a, b)>0) {separartingAxesNum++; lastSeparartingAxis=13;}
if(boundingBoxSeparationTest(vectorCross(a3, b3), a, b)>0) {separartingAxesNum++; lastSeparartingAxis=14;}
if(separartingAxesNum>1) // is not colliding yet
{
ai = a;
bi = b;
}
if(separartingAxesNum<1) // is already colliding
{
af = a;
bf = b;
}
}
if(lastSeparartingAxis<6) // face-vertex
{
struct Vector center;
int pointId = 0;
struct Vector verticles[6];
if(lastSeparartingAxis<3) // b-vertex collide with a-face
{
boundingBoxGetVerticles(verticles, &b);
center = a.position;
}
else // a-vertex collide with b-face
{
boundingBoxGetVerticles(verticles, &a);
center = b.position;
}
switch(lastSeparartingAxis)
{
case 0:
*normal = a.direction;
distance = a.halfSize.x;
break;
case 1:
*normal = a.up;
distance = a.halfSize.y;
break;
case 2:
*normal = vectorCross(a.direction, a.up);
distance = a.halfSize.z;
break;
case 3:
*normal = b.direction;
distance = b.halfSize.x;
break;
case 4:
*normal = b.up;
distance = b.halfSize.y;
break;
case 5:
*normal = vectorCross(b.direction, b.up);;
distance = b.halfSize.z;
break;
default:
*normal = vectorZero();
distance = 0;
}
minDistance = -1;
for(i=0;i<6;i++)
{
distance = vectorPointPlaneDistance(verticles[i], center, *normal);
if(minDistance < 0 || distance < minDistance)
{
minDistance = distance;
pointId = i;
}
}
*point = verticles[pointId];
}
else // edge-edge
{
struct Vector a1 = a.direction = vectorNormalize(a.direction);
struct Vector a2 = a.up = vectorNormalize(a.up);
struct Vector a3 = vectorNormalize( vectorCross(a1, a2) );
struct Vector b1 = b.direction = vectorNormalize(b.direction);
struct Vector b2 = b.up = vectorNormalize(b.up);
struct Vector b3 = vectorNormalize(vectorCross(b1, b2));
struct Vector halfEdgeA, halfEdgeB;
struct Vector halfOffsetA[4];
struct Vector halfOffsetB[4];
for(i=0;i<4;i++)
{switch(lastSeparartingAxis)
{
case 6: case 7: case 8:
halfEdgeA = vectorTimes(a1, a.halfSize.x);
halfOffsetA[i] = vectorAdd( vectorTimes(a1, 0),
vectorAdd( vectorTimes(a2, a.halfSize.y * (1-2*(i%2)) ),
vectorTimes(a3, a.halfSize.z * (i>=2 ? -1 : 1) )));
break;
case 9: case 10: case 11:
halfEdgeA = vectorTimes(a2, a.halfSize.y);
halfOffsetA[i] = vectorAdd( vectorTimes(a1, a.halfSize.x * (1-2*(i%2))),
vectorAdd( vectorTimes(a2, 0),
vectorTimes(a3, a.halfSize.z * (i>=2 ? -1 : 1))));
break;
case 12: case 13: case 14:
halfEdgeA = vectorTimes(a3, a.halfSize.z);
halfOffsetA[i] = vectorAdd( vectorTimes(a1, a.halfSize.x * (1-2*(i%2))),
vectorAdd( vectorTimes(a2, a.halfSize.y * (i>=2 ? -1 : 1)),
vectorTimes(a3, 0)));
break;
}
switch(lastSeparartingAxis)
{
case 6: case 9: case 12:
halfEdgeB = vectorTimes(b1, b.halfSize.x);
halfOffsetB[i] = vectorAdd( vectorTimes(b1, 0),
vectorAdd( vectorTimes(b2, b.halfSize.y * (1-2*(i%2))),
vectorTimes(b3, b.halfSize.z * (i>=2 ? -1 : 1))));
break;
case 7: case 10: case 13:
halfEdgeB = vectorTimes(b2, b.halfSize.y);
halfOffsetB[i] = vectorAdd( vectorTimes(b1, b.halfSize.x * (1-2*(i%2))),
vectorAdd( vectorTimes(b2, 0),
vectorTimes(b3, b.halfSize.z * (i>=2 ? -1 : 1))));
break;
case 8: case 11: case 14:
halfEdgeB = vectorTimes(b3, b.halfSize.z);
halfOffsetB[i] = vectorAdd( vectorTimes(b1, b.halfSize.x * (1-2*(i%2))),
vectorAdd( vectorTimes(b2, b.halfSize.y * (i>=2 ? -1 : 1)),
vectorTimes(b3, 0)));
break;
}
}
*normal = vectorNormalize( vectorCross(halfEdgeA, halfEdgeB) );
int edgeAi, edgeBi;
int edgeA, edgeB;
minDistance = -1;
for(edgeAi = 0; edgeAi<4; edgeAi++)
for(edgeBi = 0; edgeBi<4; edgeBi++)
{
distance = vectorDot(*normal, vectorAdd(a.position, halfOffsetA[edgeAi])) - vectorDot(*normal, vectorAdd(b.position, halfOffsetB[edgeBi]));
distance = fabs(distance);
if(minDistance == -1 || distance < minDistance)
{
minDistance = distance;
edgeA = edgeAi;
edgeB = edgeBi;
}
}
struct Vector pointEdgeA = vectorAdd(a.position, halfOffsetA[edgeA]);
struct Vector pointEdgeB = vectorAdd(b.position, halfOffsetB[edgeB]);
struct Vector rayVector = vectorNormalize(halfEdgeA);
struct Vector rayPoint = pointEdgeA;
struct Vector planePoint = pointEdgeB;
struct Vector planeNormal = vectorNormalize(vectorCross(vectorNormalize(halfEdgeB), *normal));
// ray - plane intersection
double cosAlpha = vectorDot(rayVector, planeNormal);
double distance = vectorPointPlaneDistance(rayPoint, planePoint, planeNormal);
struct Vector pointA = vectorAdd(rayPoint, vectorTimes(rayVector, cosAlpha*distance)) ;
rayVector = vectorNormalize(halfEdgeB);
rayPoint = pointEdgeB;
planePoint = pointEdgeA;
planeNormal = vectorNormalize(vectorCross(vectorNormalize(halfEdgeA), *normal));
// ray - plane intersection
cosAlpha = vectorDot(rayVector, planeNormal);
distance = vectorPointPlaneDistance(rayPoint, planePoint, planeNormal);
struct Vector pointB = vectorAdd(rayPoint, vectorTimes(rayVector, cosAlpha*distance)) ;
*point = vectorTimes(vectorAdd(pointA, pointB),0.5);
}
/* normal should point in direction from A to B*/
double smallValue = 0.001;
if(vectorLength(vectorSub(vectorAdd(*point,vectorTimes(*normal, smallValue)), a.position))
<
vectorLength(vectorSub(vectorAdd(*point,vectorZero()), a.position))) /* |point + normal - A| < |point-A| */
*normal = vectorTimes(*normal, -1.0);
*normal = vectorNormalize(*normal);
}
Vertex::Vertex(float x, float y, float z) {
vectorSet(position, x, y, z);
vectorSet(normal, x, y, z);
vectorNormalize(normal);
}