/* Returns Positive on separation - gives distance*/
double boundingBoxSeparationTest(struct Vector L, struct BoundingBox a, struct BoundingBox b)
{
double R = fabs(vectorDot(L, vectorSub(a.position, b.position)));
double R0 = a.halfSize.x*fabs(vectorDot(L, a.direction)) +
a.halfSize.y*fabs(vectorDot(L, a.up)) +
a.halfSize.z*fabs(vectorDot(L, vectorCross(a.up, a.direction)));
double R1 = b.halfSize.x*fabs(vectorDot(L, b.direction)) +
b.halfSize.y*fabs(vectorDot(L, b.up)) +
b.halfSize.z*fabs(vectorDot(L, vectorCross(b.up, b.direction)));
return R - (R0 + R1);
}
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;
}
Vector Biot_Savart_SingleLineSeg(LineSeg seg,Vector fieldpoint)
{
Vector result,r,Seg_Cross_r;
Vector SegMidPoint,SegVector,SegDirection;
Vector BDirection;
double SegLength,x,z,constval;
double Bmag;
SegMidPoint=linesegCenter(seg);
SegLength=linesegLength(seg);
SegVector=linesegVector(seg);
SegDirection=vectorUnitVector(SegVector);
r=linesegVector(linesegGenerate(SegMidPoint,fieldpoint));
Seg_Cross_r=vectorCross(SegVector,r);
BDirection=vectorUnitVector(Seg_Cross_r);
z=vectorMagnitude(Seg_Cross_r)/SegLength;
constval=mu_0/(4*M_PI*z);
x=vectorDot(SegDirection,r);
if(z == 0)
{
Bmag=0;
}
else
{
Bmag=constval*((SegLength-2*x)/sqrt((SegLength-2*x)*(SegLength-2*x)+4*z*z)+(SegLength+2*x)/sqrt((SegLength+2*x)*(SegLength+2*x)+4*z*z));
}
//printf("Bmag %g\n",Bmag);
result=vectorScale(BDirection,Bmag);
//printf("result.x,result.y,result.z %g,%g,%g\n",result.x,result.y,result.z);
return result;
}
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;
}
void vectorMulMatrixCoords(struct Vector u, struct Vector v)
{
double m[16];
struct Vector w = vectorCross(u ,v);
m[0] = u.x;
m[1] = u.y;
m[2] = u.z;
m[3] = 0.0;
m[4] = v.x;
m[5] = v.y;
m[6] = v.z;
m[7] = 0.0;
m[8] = w.x;
m[9] = w.y;
m[10] = w.z;
m[11] = 0.0;
m[12] = 0.0;
m[13] = 0.0;
m[14] = 0.0;
m[15] = 1.0;
glMultMatrixd(m);
glRotatef(-90.0,0,1,0);
}
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;
}
bool pointInPoly (const VECTOR3 * const point, int points, const VECTOR3 * const poly)
{
VECTOR3
u, v,
plane;
enum dim_drop
drop;
int
i = 0,
side,
sideMatch = 10;
float
x, y,
px[2], py[2];
if (points < 3)
return false;
u = vectorSubtract (&poly[1], &poly[0]);
v = vectorSubtract (&poly[2], &poly[0]);
plane = vectorCross (&u, &v);
drop =
(fabs(plane.x) > fabs(plane.y))
? (fabs(plane.x) > fabs(plane.z))
? DROP_X
: DROP_Z
: (fabs(plane.y) > fabs(plane.z))
? DROP_Y
: DROP_Z;
//printf ("dropping %c\n", drop == DROP_X ? 'X' : drop == DROP_Y ? 'Y' : 'Z');
x = (drop == DROP_X) ? point->y : point->x;
y = (drop == DROP_Z) ? point->y : point->z;
px[1] = (drop == DROP_X) ? poly[points-1].y : poly[points-1].x;
py[1] = (drop == DROP_Z) ? poly[points-1].y : poly[points-1].z;
while (i < points)
{
px[0] = px[1];
py[0] = py[1];
px[1] = (drop == DROP_X) ? poly[i].y : poly[i].x;
py[1] = (drop == DROP_Z) ? poly[i].y : poly[i].z;
side = turns (x, y, px[0], py[0], px[1], py[1]);
//printf ("%.2f, %.2f vs. %.2f, %.2f -> %.2f, %.2f: %c\n", x, y, px[0], py[0], px[1], py[1], d == RIGHT ? 'R' : d == LEFT ? 'L' : 'T');
if (sideMatch != 10 && side != sideMatch)
return false;
sideMatch = side;
i++;
}
return true;
}
//verticles in global frame of reference
void boundingBoxGetVerticles(struct Vector *verticles, const struct BoundingBox *bb)
{
verticles[0]=vectorAdd(bb->position,
vectorAdd( vectorTimes(bb->direction, bb->halfSize.x),
vectorAdd( vectorTimes(bb->up, bb->halfSize.y),
vectorTimes( vectorCross(bb->up, bb->direction), bb->halfSize.z)
)));
verticles[1]=vectorAdd(bb->position,
vectorAdd( vectorTimes(bb->direction, -bb->halfSize.x),
vectorAdd( vectorTimes(bb->up, bb->halfSize.y),
vectorTimes( vectorCross(bb->up, bb->direction), bb->halfSize.z)
)));
verticles[2]=vectorAdd(bb->position,
vectorAdd( vectorTimes(bb->direction, -bb->halfSize.x),
vectorAdd( vectorTimes(bb->up, -bb->halfSize.y),
vectorTimes( vectorCross(bb->up, bb->direction), bb->halfSize.z)
)));
verticles[3]=vectorAdd(bb->position,
vectorAdd( vectorTimes(bb->direction, bb->halfSize.x),
vectorAdd( vectorTimes(bb->up, -bb->halfSize.y),
vectorTimes( vectorCross(bb->up, bb->direction), bb->halfSize.z)
)));
verticles[4]=vectorAdd(bb->position,
vectorAdd( vectorTimes(bb->direction, bb->halfSize.x),
vectorAdd( vectorTimes(bb->up, bb->halfSize.y),
vectorTimes( vectorCross(bb->up, bb->direction), -bb->halfSize.z)
)));
verticles[5]=vectorAdd(bb->position,
vectorAdd( vectorTimes(bb->direction, -bb->halfSize.x),
vectorAdd( vectorTimes(bb->up, bb->halfSize.y),
vectorTimes( vectorCross(bb->up, bb->direction), -bb->halfSize.z)
)));
verticles[6]=vectorAdd(bb->position,
vectorAdd( vectorTimes(bb->direction, -bb->halfSize.x),
vectorAdd( vectorTimes(bb->up, -bb->halfSize.y),
vectorTimes( vectorCross(bb->up, bb->direction), -bb->halfSize.z)
)));
verticles[7]=vectorAdd(bb->position,
vectorAdd( vectorTimes(bb->direction, bb->halfSize.x),
vectorAdd( vectorTimes(bb->up, -bb->halfSize.y),
vectorTimes( vectorCross(bb->up, bb->direction), -bb->halfSize.z)
)));
}
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);
}
}
}
bool line_triHit (const LINE3 * const line, const VECTOR3 * const tri, float * tAtIntersection)
{
VECTOR3
u, v,
triPlane,
hit;
LINE3
transLine;
float
t,
weight[3];
u = vectorSubtract (&tri[2], &tri[0]);
v = vectorSubtract (&tri[1], &tri[0]);
triPlane = vectorCross (&u, &v);
// the planeHit code doesn't have a magnitude argument so it's always a test against the plane through the origin. thus to accurately test we have to translate the line so that it's positioned relative to the plane origin (which we're arbitrarily using tri->pts[0] as) - xph 02 13 2012
transLine.origin = vectorSubtract (&line->origin, &tri[0]);
transLine.dir = line->dir;
if (!line_planeHit (&transLine, &triPlane, &t))
return false;
hit = vectorCreate
(
transLine.origin.x + line->dir.x * t,
transLine.origin.y + line->dir.y * t,
transLine.origin.z + line->dir.z * t
);
baryWeights (&hit, &u, &v, weight);
if (weight[0] >= 0.00 && weight[0] <= 1.00 &&
weight[1] >= 0.00 && weight[1] <= 1.00 &&
weight[2] >= 0.00 && weight[2] <= 1.00)
{
if (tAtIntersection)
*tAtIntersection = t;
return true;
}
return false;
}
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 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);
}
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;
}
