/*
***************************************************************
*
* Draws the face normals on the weapon model
*
***************************************************************
*/
void GLWidget::drawWeaponFaceNormals()
{
glDisable(GL_LIGHTING);
glColor3f(0.0, 0.0, 1.0);
for(int i = 0; i < weaponReader_.numberOfTriangles(); i++)
{
int indexOne = 0;
int indexTwo = 0;
int indexThree = 0;
weaponReader_.retrieveTriangleVertexIndicies(i, &indexOne,
&indexTwo, &indexThree);
MathVector* faceNormals = weaponReader_.faceNormals()->at(i);
VertexCoordinate vertexOne = weaponReader_.retrieveVertexCoordinatesAt(indexOne);
VertexCoordinate vertexTwo = weaponReader_.retrieveVertexCoordinatesAt(indexTwo);
VertexCoordinate vertexThree = weaponReader_.retrieveVertexCoordinatesAt(indexThree);
VertexCoordinate middleOfTriangle;
middleOfTriangle.x = ((vertexOne.x + vertexTwo.x + vertexThree.x)/3);
middleOfTriangle.y = ((vertexOne.y + vertexTwo.y + vertexThree.y)/3);
middleOfTriangle.z = ((vertexOne.z + vertexTwo.z + vertexThree.z)/3);
glBegin(GL_LINES);
glVertex3f(middleOfTriangle.x, middleOfTriangle.y,
middleOfTriangle.z);
glVertex3f((faceNormals->x()*2)+middleOfTriangle.x, (faceNormals->y()*2)+middleOfTriangle.y,
(faceNormals->z()*2)+middleOfTriangle.z);
glEnd();
}
glEnable(GL_LIGHTING);
}
MathVector tripleProduct(MathVector a, MathVector b, MathVector c)
{
MathVector U = b.multiply(c.dotProduct(a));
MathVector V = a.multiply(c.dotProduct(b));
return U.subtractVectors(V);
}
void CBaseBrushDraw::UpdateControlPoints()
{
GetBrushGridIndexs(m_vecBrushGridIndices, m_vecBrushVertexs);
if ( m_lastBrushVertexs == m_vecBrushVertexs )
return;
m_lastBrushVertexs = m_vecBrushVertexs;
int nWidth = m_nOuterWidth * 2;
MathVector<CVector3f> temp;
CVector3f leftbottom, rightbottom, righttop, lefttop;
leftbottom = m_vecBrushVertexs[0];
rightbottom = m_vecBrushVertexs[0+nWidth];
righttop = m_vecBrushVertexs[m_vecBrushVertexs.size()-1];
lefttop = m_vecBrushVertexs[m_vecBrushVertexs.size()-1-nWidth];
leftbottom.y += 1.0f;
rightbottom.y += 1.0f;
righttop.y += 1.0f;
leftbottom.y += 1.0f;
temp.push_back(leftbottom);
temp.push_back(rightbottom);
temp.push_back(righttop);
temp.push_back(lefttop);
m_curveSampler.SetControlPoint(temp, true);
}
MathVector Math::normal(MathVector a, MathVector b, MathVector c)
{
MathVector r1;
MathVector r2;
MathVector output;
r2=c-b;
r1=b-a;
output = r1.cross(r2);
output=(1.0f/output.size())*output;
return output;
}
void change_route( MathVector mSource, MathVector mDestination )
{
printf( "Now Showing Route : \n" );
mSource.print();
mDestination.print();
map2D.map_route ( multiRoute, mSource, mDestination );
//robot.m_route.create_from_multi( multiRoute );
//robot.plan_steps ( 2.0*12 );
robot.gl_register ( );
robot.m_glide_index = 0;
}
const MathVector<T, dimension> reflected(const MathVector<T, dimension>& other) const {
MathVector<T, dimension> output;
output = (*this) - other * T(2.0) * other.dot(*this);
return output;
}
void Particle::updateVelocity()
{
if (parameters.fips)
{
MathVector influence;
influence.fillValues(v.size(), 0.0);
for(auto n: neighbours)
influence = influence + randDouble(0.0, parameters.c) * (n->p - x);
influence = (1.0 / neighbours.size()) * influence;
v = parameters.w * v + influence;
}
else
{
v = parameters.w * v + randDouble(0.0, parameters.c) * (p - x) + randDouble(0.0, parameters.c) * (best->p - x);
}
}
bool containsOrigin(std::vector<MathVector> &simplex, MathVector &direction)
{
MathVector a = simplex.back();
MathVector b, c, ab, ac, abPerp, acPerp;
MathVector ao = a.negate();
if(simplex.size() == 3)
{
b = simplex[0];
c = simplex[1];
ab = b.subtractVectors(a);
ac = c.subtractVectors(a);
abPerp = tripleProduct(ac, ab, ab);
acPerp = tripleProduct(ab, ac, ac);
if(abPerp.dotProduct(ao) > 0)
{
simplex.erase(simplex.begin() + 1);
direction = abPerp;
} else if (acPerp.dotProduct(ao) > 0)
{
simplex.erase(simplex.begin());
direction = acPerp;
} else
{
return true;
}
} else
{
b = simplex[0];
ab = b.subtractVectors(a);
abPerp = tripleProduct(ab, ao, ab);
if(abPerp.getX() == 0 || abPerp.getY() == 0)
{
direction = ab.perpendicular();
} else
{
direction = abPerp;
}
}
return false;
}
/*
***************************************************************
*
* Draws the vertex normals on the weapon model
*
***************************************************************
*/
void GLWidget::drawWeaponVertexNormals()
{
glDisable(GL_LIGHTING);
glColor3f(1.0, 0.0, 0.0);
for(int i = 0; i < weaponReader_.numberOfVertices(); i++)
{
MathVector* vertexNormal = weaponReader_.vertexNormals()->at(i);
VertexCoordinate vertexCoordinate = weaponReader_.retrieveVertexCoordinatesAt(i);
glBegin(GL_LINES);
glVertex3f(vertexCoordinate.x, vertexCoordinate.y,
vertexCoordinate.z);
glVertex3f((vertexNormal->x()*2)+vertexCoordinate.x, (vertexNormal->y()*2)+vertexCoordinate.y,
(vertexNormal->z()*2)+vertexCoordinate.z);
glEnd();
}
glEnable(GL_LIGHTING);
}
minkowskiDifference_t buildMinkowskiDifference(std::vector<MathVector> a, std::vector<MathVector> b)
{
MathVector direction = MathVector(1,1);
std::vector<MathVector> simplex;
simplex.push_back(getSupportVertex(a, b, direction));
minkowskiDifference_t difference;
direction = direction.negate();
while(true)
{
simplex.push_back(getSupportVertex(a, b, direction));
if(simplex.back().dotProduct(direction) <= 0)
{
difference.colliding = false;
difference.collisionNormal = MathVector(0,0);
difference.collisionDepth = 0;
return difference;
} else if(containsOrigin(simplex, direction) && simplex.size() == 3)
{
while(true)
{
//Perform EPA to get collision normal and penetration distance
Edge_t e = findClosestEdge(simplex);
MathVector p = getSupportVertex(a, b, direction);
double d = p.dotProduct(e.normal);
// std::cout << d - e.distance << std::endl;
// std::cout << "Simplex size: " << simplex.size() << std::endl;
if(d - e.distance < TOLERANCE)
{
difference.collisionNormal = e.normal;
difference.collisionDepth = d;
difference.colliding = true;
return difference;
} else
{
// std::cout << "Closest edge not found in this iteration, adding point to simplex and continuing." << std::endl;
simplex.insert((simplex.begin()+e.index),p);
}
}
}
}
}
void move_sideways( float mAmount )
{
MathVector forward(3);
forward[0] = centerX - eyeX;
forward[1] = centerY - eyeY;
forward[2] = centerZ - eyeZ;
MathVector perp = forward.get_perp_xz();
perp.unitize();
perp *= mAmount;
eyeX += perp[0];
eyeZ += perp[2];
centerX += perp[0];
centerZ += perp[2];
theWorld.look_at( eyeX, eyeY, eyeZ,
centerX, centerY, centerZ,
0.0, 1.0, 0.0 );
}
// overloaded multiplication of matrix by a vector
MathVector MathMatrix::operator*(const MathVector& v) const
{
if (n != v.size()) { throw "Matrix and Vector do not"; }
//Create matrix object of the correct size to hold the resulting matrix from multiplication
MathVector temp(v.size());
//Go across the rows of the matrix the method was called on
for (int i=0; i<nrows; i++)
{
//Declare empty variable to hold the multiplication result
double sum = 0;
//Go across the columns on the matrix that was passed as a parameter
for (int j=0; j<ncols; j++)
{
sum+=(*this)(i, j) * v[0];
}
//Set element in temp at the corresponding loop iteration index to the result
temp[i] = sum;
}
return temp;
}
MathVector getFurthestPoint(MathVector direction, std::vector<MathVector> polygon)
{
double greatestDotProduct = -std::numeric_limits<double>::max();
double currentDotProduct;
MathVector currentVertex;
MathVector bestVertex;
for(int i = 0; i < polygon.size(); i++)
{
currentVertex = polygon[i];
currentDotProduct = currentVertex.dotProduct(direction);
if(currentDotProduct > greatestDotProduct)
{
greatestDotProduct = currentDotProduct;
bestVertex = currentVertex;
}
}
return bestVertex;
}
Quat Math::rot2Quat( MathVector in)
{
MathVector inv = in;
Quat nq;
double invs;
double phi;
MathVector unitV;
invs = inv.size();
if(invs != 0)
{
unitV = 1/invs * inv ;
}
else
{
unitV = MathVector(1,0,0);
}
phi = invs * 3.14159265/180;
nq.v = sin(phi/2) * unitV;
nq.scale = cos(phi/2);
return nq;
}
void sim_read_robot_angles( long& mRobot_id, MathVector& mNewAngles )
{
if (ipc_memory_valid_pointer()==false)
throw IPC_Error;
if (ipc_memory_sim->Command == COMMAND_ROBOT_ANGLES)
{
mRobot_id = ipc_memory_sim->robot_id;
// Dimension the new MathVector:
int dimension = (ipc_memory_sim->object_datum1 & 0xFF);
if (dimension> MAX_SERVOS) dimension = MAX_SERVOS;
mNewAngles.dimension(dimension);
// EXTRACT the angles:
for (int i=0; i<dimension; i++)
mNewAngles[i] = ipc_memory_sim->servo_angles[i];
}
}
typename MathVector<T>::value_type operator*(const MathVector<T>& lhs, const MathVector<T>& rhs)
{
return lhs.dotProduct(rhs);
}
bool Bezier::intersectsQuadrilateral(const MathVector<float, 3>& orig, const MathVector<float, 3>& dir,
const MathVector<float, 3>& v_00, const MathVector<float, 3>& v_10,
const MathVector<float, 3>& v_11, const MathVector<float, 3>& v_01,
float &t, float &u, float &v) const {
const float EPSILON = 0.000001;
// Reject rays that are parallel to Q, and rays that intersect the plane
// of Q either on the left of the line V00V01 or below the line V00V10.
MathVector<float, 3> E_01 = v_10 - v_00;
MathVector<float, 3> E_03 = v_01 - v_00;
MathVector<float, 3> P = dir.cross(E_03);
float det = E_01.dot(P);
if (std::abs(det) < EPSILON) return false;
MathVector<float, 3> T = orig - v_00;
float alpha = T.dot(P) / det;
if (alpha < 0.0) return false;
MathVector<float, 3> Q = T.cross(E_01);
float beta = dir.dot(Q) / det;
if (beta < 0.0) return false;
if (alpha + beta > 1.0) {
// Reject rays that that intersect the plane of Q either on
// the right of the line V11V10 or above the line V11V00.
MathVector<float, 3> E_23 = v_01 - v_11;
MathVector<float, 3> E_21 = v_10 - v_11;
MathVector<float, 3> P_prime = dir.cross(E_21);
float det_prime = E_23.dot(P_prime);
if (std::abs(det_prime) < EPSILON) return false;
MathVector<float, 3> T_prime = orig - v_11;
float alpha_prime = T_prime.dot(P_prime) / det_prime;
if (alpha_prime < 0.0) return false;
MathVector<float, 3> Q_prime = T_prime.cross(E_23);
float beta_prime = dir.dot(Q_prime) / det_prime;
if (beta_prime < 0.0) return false;
}
// Compute the ray parameter of the intersection point, and
// reject the ray if it does not hit Q.
t = E_03.dot(Q) / det;
if (t < 0.0) return false;
// Compute the barycentric coordinates of the fourth vertex.
// These do not depend on the ray, and can be precomputed
// and stored with the quadrilateral.
float alpha_11, beta_11;
MathVector<float, 3> E_02 = v_11 - v_00;
MathVector<float, 3> n = E_01.cross(E_03);
if ((std::abs(n[0]) >= std::abs(n[1])) && (std::abs(n[0]) >= std::abs(n[2]))) {
alpha_11 = ((E_02[1] * E_03[2]) - (E_02[2] * E_03[1])) / n[0];
beta_11 = ((E_01[1] * E_02[2]) - (E_01[2] * E_02[1])) / n[0];
} else if ((std::abs(n[1]) >= std::abs(n[0])) && (std::abs(n[1]) >= std::abs(n[2]))) {
alpha_11 = ((E_02[2] * E_03[0]) - (E_02[0] * E_03[2])) / n[1];
beta_11 = ((E_01[2] * E_02[0]) - (E_01[0] * E_02[2])) / n[1];
} else {
alpha_11 = ((E_02[0] * E_03[1]) - (E_02[1] * E_03[0])) / n[2];
beta_11 = ((E_01[0] * E_02[1]) - (E_01[1] * E_02[0])) / n[2];
}
// Compute the bilinear coordinates of the intersection point.
if (std::abs(alpha_11 - (1.0)) < EPSILON) {
// Q is a trapezium.
u = alpha;
if (std::abs(beta_11 - (1.0)) < EPSILON) v = beta; // Q is a parallelogram.
else v = beta / ((u * (beta_11 - (1.f))) + (1.f)); // Q is a trapezium.
} else if (std::abs(beta_11 - (1.0)) < EPSILON) {
// Q is a trapezium.
v = beta;
if ( ((v * (alpha_11 - (1.0))) + (1.0)) == 0 ) return false;
u = alpha / ((v * (alpha_11 - (1.f))) + (1.f));
} else {
float A = (1.f) - beta_11;
float B = (alpha * (beta_11 - (1.f)))
- (beta * (alpha_11 - (1.f))) - (1.f);
float C = alpha;
float D = (B * B) - ((4.f) * A * C);
if (D < 0) return false;
float Q = (-0.5) * (B + ((B < (0.0) ? (-1.0) : (1.0))
* std::sqrt(D)));
u = Q / A;
if ((u < (0.0)) || (u > (1.0))) u = C / Q;
v = beta / ((u * (beta_11 - (1.f))) + (1.f));
}
return true;
}
/*
***************************************************************
*
* Draws the model based on the opened MD2 model file
*
***************************************************************
*/
void GLWidget::drawModel()
{
switch(displayMode_)
{
case DrawingDefines::WIREFRAME:
{
glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
break;
}
case DrawingDefines::FLAT_SHADING:
{
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
glShadeModel(GL_FLAT);
break;
}
case DrawingDefines::SMOOTH_SHADING:
{
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
glShadeModel(GL_SMOOTH);
break;
}
}
if(textureLoadedForMd2Model_)
{
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D, modelTexture_);
}
else
{
glColor3f(0.0, 1.0, 0.0);
}
glBegin(GL_TRIANGLES);
for(int currentTriangle = 0;
currentTriangle < modelReader_.numberOfTriangles(); currentTriangle++)
{
int indexOne = 0;
int indexTwo = 0;
int indexThree = 0;
modelReader_.retrieveTriangleVertexIndicies(currentTriangle, &indexOne,
&indexTwo, &indexThree);
int texOne = 0;
int texTwo = 0;
int texThree = 0;
modelReader_.retrieveTriangleTextureIndicies(currentTriangle, &texOne,
&texTwo, &texThree);
VertexCoordinate vertexOne = modelReader_.retrieveVertexCoordinatesAt(indexOne);
VertexCoordinate vertexTwo = modelReader_.retrieveVertexCoordinatesAt(indexTwo);
VertexCoordinate vertexThree = modelReader_.retrieveVertexCoordinatesAt(indexThree);
TextureCoordinate textureOne = modelReader_.retrieveTextureCoordinateAt(texOne);
TextureCoordinate textureTwo = modelReader_.retrieveTextureCoordinateAt(texTwo);
TextureCoordinate textureThree = modelReader_.retrieveTextureCoordinateAt(texThree);
//Point One.
MathVector* vector;
if(displayMode_== DrawingDefines::FLAT_SHADING)
{
vector = modelReader_.faceNormals()->at(currentTriangle);
}
else
{
vector = modelReader_.vertexNormals()->at(indexOne);
}
glNormal3f(vector->x(), vector->y(), vector->z());
glTexCoord2f((float) textureOne.u/modelReader_.skinWidth(),
(float) textureOne.v/modelReader_.skinHeight());
glVertex3f(vertexOne.x, vertexOne.y, vertexOne.z);
//Point Two
if(displayMode_== DrawingDefines::FLAT_SHADING)
{
vector = modelReader_.faceNormals()->at(currentTriangle);
}
else
{
vector = modelReader_.vertexNormals()->at(indexTwo);
}
glNormal3f(vector->x(), vector->y(), vector->z());
glTexCoord2f((float) textureTwo.u/modelReader_.skinWidth(),
(float) textureTwo.v/modelReader_.skinHeight());
glVertex3f(vertexTwo.x, vertexTwo.y, vertexTwo.z);
//Point Three
if(displayMode_== DrawingDefines::FLAT_SHADING)
{
vector = modelReader_.faceNormals()->at(currentTriangle);
}
else
{
vector = modelReader_.vertexNormals()->at(indexThree);
}
glNormal3f(vector->x(), vector->y(), vector->z());
glTexCoord2f((float) textureThree.u/modelReader_.skinWidth(),
(float) textureThree.v/modelReader_.skinHeight());
glVertex3f(vertexThree.x, vertexThree.y, vertexThree.z);
}
glEnd();
glDisable(GL_TEXTURE_2D);
if(weaponLoaded_)
{
drawWeapon();
}
}
MathVector MathVector::rotate(double angle, MathVector around) {
MathVector me = *this;
return me * cos(angle) + around * ((1 - cos(angle)) * me.scalarP(around)) + around.vectP(me) * sin(angle);
}
void Bezier::setFromCorners(const MathVector<float, 3>& fl, const MathVector<float, 3>& fr,
const MathVector<float, 3>& bl, const MathVector<float, 3>& br) {
MathVector<float, 3> temp;
center = fl + fr + bl + br; center = center * 0.25;
radius = 0;
if ((fl - center).magnitude() > radius) radius = (fl - center).magnitude();
if ((fr - center).magnitude() > radius) radius = (fr - center).magnitude();
if ((bl - center).magnitude() > radius) radius = (bl - center).magnitude();
if ((br - center).magnitude() > radius) radius = (br - center).magnitude();
// Assign corners
points[0][0] = fl;
points[0][3] = fr;
points[3][3] = br;
points[3][0] = bl;
// Calculate intermediate front and back points
temp = fr - fl;
if (temp.magnitude() < 0.0001) {
points[0][1] = fl;
points[0][2] = fl;
} else {
points[0][1] = fl + temp.normalized() * (temp.magnitude() / 3.0);
points[0][2] = fl + temp.normalized() * (2.0 * temp.magnitude() / 3.0);
}
temp = br - bl;
if (temp.magnitude() < 0.0001) {
points[3][1] = bl;
points[3][2] = bl;
} else {
points[3][1] = bl + temp.normalized() * (temp.magnitude() / 3.0);
points[3][2] = bl + temp.normalized() * (2.0 * temp.magnitude() / 3.0);
}
// Calculate intermediate left and right points
int i;
for (i = 0; i < 4; ++i) {
temp = points[3][i] - points[0][i];
if (temp.magnitude() > 0.0001) {
points[1][i] = points[0][i] + temp.normalized() * (temp.magnitude() / 3.0);
points[2][i] = points[0][i] + temp.normalized() * (2.0 * temp.magnitude() / 3.0);
} else {
points[1][i] = points[0][i];
points[2][i] = points[0][i];
}
}
}
double Math::size(MathVector in)
{
return in.size();
}
// Project this vector onto the vector 'vec'
MathVector<T,3> project(const MathVector<T,3>& vec) const {
T scalarProj = dot(vec.normalized());
return vec.normalized() * scalarProj;
}
// Return the reflection of this vector around the given normal (must be unit length)
const MathVector<T,3> reflect(const MathVector<T,3> & other) const {
return (*this) - other * T(2.0) *other.dot(*this);
}
double Math::distance(MathVector a, MathVector b)
{
MathVector delta = a - b;
return delta.size();
}
MathVector getSupportVertex(std::vector<MathVector> a, std::vector<MathVector> b, MathVector direction)
{
MathVector point0 = getFurthestPoint(direction,a);
MathVector point1 = getFurthestPoint(direction.negate(),b);
return point0.subtractVectors(point1);
}
/*
***************************************************************
*
* Draws the weapon model that is currently opened in the weaponReader_
*
***************************************************************
*/
void GLWidget::drawWeapon()
{
if(textureLoadedForWeapon_)
{
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D, weaponTexture_);
}
else
{
glColor3f(1.0, 0.0, 0.0);
}
glBegin(GL_TRIANGLES);
for(int currentTriangle = 0;
currentTriangle < weaponReader_.numberOfTriangles(); currentTriangle++)
{
int indexOne = 0;
int indexTwo = 0;
int indexThree = 0;
weaponReader_.retrieveTriangleVertexIndicies(currentTriangle, &indexOne,
&indexTwo, &indexThree);
int texOne = 0;
int texTwo = 0;
int texThree = 0;
weaponReader_.retrieveTriangleTextureIndicies(currentTriangle, &texOne,
&texTwo, &texThree);
VertexCoordinate vertexOne = weaponReader_.retrieveVertexCoordinatesAt(indexOne);
VertexCoordinate vertexTwo = weaponReader_.retrieveVertexCoordinatesAt(indexTwo);
VertexCoordinate vertexThree = weaponReader_.retrieveVertexCoordinatesAt(indexThree);
TextureCoordinate textureOne = weaponReader_.retrieveTextureCoordinateAt(texOne);
TextureCoordinate textureTwo = weaponReader_.retrieveTextureCoordinateAt(texTwo);
TextureCoordinate textureThree = weaponReader_.retrieveTextureCoordinateAt(texThree);
//Point One.
MathVector* vector;
if(displayMode_== DrawingDefines::FLAT_SHADING)
{
vector = weaponReader_.faceNormals()->at(currentTriangle);
}
else
{
vector = weaponReader_.vertexNormals()->at(indexOne);
}
glNormal3f(vector->x(), vector->y(), vector->z());
glTexCoord2f((float) textureOne.u/weaponReader_.skinWidth(),
(float) textureOne.v/weaponReader_.skinHeight());
glVertex3f(vertexOne.x, vertexOne.y, vertexOne.z);
//Point Two
if(displayMode_== DrawingDefines::FLAT_SHADING)
{
vector = weaponReader_.faceNormals()->at(currentTriangle);
}
else
{
vector = weaponReader_.vertexNormals()->at(indexTwo);
}
glNormal3f(vector->x(), vector->y(), vector->z());
glTexCoord2f((float) textureTwo.u/weaponReader_.skinWidth(),
(float) textureTwo.v/weaponReader_.skinHeight());
glVertex3f(vertexTwo.x, vertexTwo.y, vertexTwo.z);
//Point Three
if(displayMode_== DrawingDefines::FLAT_SHADING)
{
vector = weaponReader_.faceNormals()->at(currentTriangle);
}
else
{
vector = weaponReader_.vertexNormals()->at(indexThree);
}
vector = weaponReader_.vertexNormals()->at(indexThree);
glNormal3f(vector->x(), vector->y(), vector->z());
glTexCoord2f((float) textureThree.u/weaponReader_.skinWidth(),
(float) textureThree.v/weaponReader_.skinHeight());
glVertex3f(vertexThree.x, vertexThree.y, vertexThree.z);
}
glEnd();
glDisable(GL_TEXTURE_2D);
}
///
// TriangleBoxIntersection()
//
// Determine if a bounding box and triangle intersect
//
bool TriangleBoxIntersection(const MathVector<3>& p0, const MathVector<3>& p1,
const MathVector<3>& p2,
const MathVector<3>& boxMin, const MathVector<3>& boxMax)
{
vector3 Trans;
vector3 Scale(1.0, 1.0, 1.0);
vector3 TransMax;
TRI TestTri;
///
// Compute the scale and transform required to make BBox
// a voxel
//
Trans.x() = (boxMax.x() + boxMin.x()) / 2;
Trans.y() = (boxMax.y() + boxMin.y()) / 2;
Trans.z() = (boxMax.z() + boxMin.z()) / 2;
VecSubtract(TransMax, boxMax, Trans);
if(TransMax.x() != 0)
Scale.x() = 0.5f / TransMax.x();
if(TransMax.y() != 0)
Scale.y() = 0.5f / TransMax.y();
if(TransMax.z() != 0)
Scale.z() = 0.5f / TransMax.z();
///
// Put the triangle in voxel space
//
TestTri.m_P[0].x() = (p0.x() - Trans.x()) * Scale.x();
TestTri.m_P[0].y() = (p0.y() - Trans.y()) * Scale.y();
TestTri.m_P[0].z() = (p0.z() - Trans.z()) * Scale.z();
TestTri.m_P[1].x() = (p1.x() - Trans.x()) * Scale.x();
TestTri.m_P[1].y() = (p1.y() - Trans.y()) * Scale.y();
TestTri.m_P[1].z() = (p1.z() - Trans.z()) * Scale.z();
TestTri.m_P[2].x() = (p2.x() - Trans.x()) * Scale.x();
TestTri.m_P[2].y() = (p2.y() - Trans.y()) * Scale.y();
TestTri.m_P[2].z() = (p2.z() - Trans.z()) * Scale.z();
///
// Test against the voxel
//
return(TriCubeIntersection(TestTri) == INSIDE);
}
MathVector Math::cross(MathVector a, MathVector b)
{
return a.cross(b);
}
