bool lineSegmentIntersect(xy_t line, xy_t dir, xy_t p1, xy_t p2, xy_t *p)
{
xy_t s;
s.x = p2.x - p1.x;
s.y = p2.y - p1.y;
xy_t diff;
diff.x = p1.x - line.x;
diff.y = p1.y - line.y;
v_t denom = vectorCrossProduct(dir, s);
v_t u = vectorCrossProduct(diff, dir);
if(fabs(denom) < V_ERROR){
if(fabs(u) < V_ERROR){
p->x = (line.x + p1.x) / 2;
p->y = (line.y + p1.y) / 2;
return true;
}else{
return false;
}
}
u /= denom;
if(u < -V_ERROR || u > 1.0 + V_ERROR){
return false;
}
p->x = p1.x + u * s.x;
p->y = p1.y + u * s.y;
return true;
}
static REAL triangleArea2( const BVector *vertexA, const BVector *vertexB, const BVector *vertexC )
{
BVector ab, ac;
vectorSubtract( vertexB, vertexA, &ab );
vectorSubtract( vertexC, vertexA, &ac );
return vectorCrossProduct( &ac, &ab );
}
Vector calculateMagneticFieldPoint(double I, double permeability, Vector l, Vector r) {
const double constant = permeability / (4 * M_PI);
const Vector r1 = vectorSubstract(r, l);
const double r1LenSq = vectorGetLengthSq(r1);
const double r1Len = sqrt(r1LenSq);
return vectorMultiply(vectorCrossProduct(l, r1), constant * I / r1LenSq / r1Len);
}
void OBJ_GenerateLightingInfo(obj_t* obj)
{
vec3_t v1,v2 ;
int i;
vec2_t st1,st2;
float coef;
vec3_t currentTangent;
vec3_t currentNormal;
vec3_t* tangents;
vec3_t* normals;
tangents = (vec3_t*)calloc(obj->num_vertices, sizeof(vec3_t));
normals = (vec3_t*)calloc(obj->num_vertices, sizeof(vec3_t));
//Accumulate tangents & normals
for(i=0;i<obj->num_indices/3;i++)
{
vectorSubtract(obj->vertices[obj->indices[i*3+2]].position , obj->vertices[obj->indices[i*3]].position, v1);
vectorSubtract(obj->vertices[obj->indices[i*3+1]].position , obj->vertices[obj->indices[i*3]].position, v2);
vector2Subtract(obj->vertices[obj->indices[i*3+2]].textCoo,obj->vertices[obj->indices[i*3]].textCoo,st1);
vector2Subtract(obj->vertices[obj->indices[i*3+1]].textCoo,obj->vertices[obj->indices[i*3]].textCoo,st2);
//Normal part
vectorCrossProduct(v2, v1, currentNormal);
vectorAdd(normals[obj->indices[i*3+0]],currentNormal,normals[obj->indices[i*3+0]]);
vectorAdd(normals[obj->indices[i*3+1]],currentNormal,normals[obj->indices[i*3+1]]);
vectorAdd(normals[obj->indices[i*3+2]],currentNormal,normals[obj->indices[i*3+2]]);
//Tangent part
coef = 1/ (st1[0] * st2[1] - st2[0] * st1[1]);
currentTangent[0] = coef * (v1[0] * st2[1] + v2[0] * -st1[1]);
currentTangent[1] = coef * (v1[1] * st2[1] + v2[1] * -st1[1]);
currentTangent[2] = coef * (v1[2] * st2[1] + v2[2] * -st1[1]);
vectorAdd(tangents[obj->indices[i*3+0]],currentTangent,tangents[obj->indices[i*3+0]]);
vectorAdd(tangents[obj->indices[i*3+1]],currentTangent,tangents[obj->indices[i*3+1]]);
vectorAdd(tangents[obj->indices[i*3+2]],currentTangent,tangents[obj->indices[i*3+2]]);
}
//Vector Normalize + Normalize
for(i=0;i<obj->num_vertices;i++)
{
normalize(tangents[i]);
vectorScale(tangents[i],DE_SHRT_MAX,obj->vertices[i].tangent);
normalize(normals[i]);
vectorScale(normals[i],DE_SHRT_MAX,obj->vertices[i].normal);
}
free(tangents);
free(normals);
}
/*
-----------------------------------------------------------------------------
Function: RotatePointAroundVector -Rotate a point around a vector.
Parameters: dst -[out] Point after rotation.
dir -[in] vector.
point -[in] Point.
degrees -[in] Degrees of rotation.
Returns: Nothing.
Notes:
-----------------------------------------------------------------------------
*/
PUBLIC void RotatePointAroundVector( vec3_t dst, const vec3_t dir, const vec3_t point, float degrees )
{
mat3_t m;
mat3_t im;
mat3_t zrot;
mat3_t tmpmat;
mat3_t rot;
vec3_t vr, vup, vf;
float rad;
vf[0] = dir[0];
vf[1] = dir[1];
vf[2] = dir[2];
PerpendicularVector( vr, dir );
vectorCrossProduct( vr, vf, vup );
m[0] = vr[0];
m[3] = vr[1];
m[6] = vr[2];
m[1] = vup[0];
m[4] = vup[1];
m[7] = vup[2];
m[2] = vf[0];
m[5] = vf[1];
m[8] = vf[2];
memcpy( im, m, sizeof( im ) );
im[1] = m[3];
im[2] = m[6];
im[3] = m[1];
im[5] = m[7];
im[6] = m[2];
im[7] = m[5];
memset( zrot, 0, sizeof( zrot ) );
zrot[0] = zrot[4] = zrot[8] = 1.0F;
rad = DEG2RAD( degrees );
zrot[0] = (float)cos( rad );
zrot[1] = (float)sin( rad );
zrot[3] = (float)-sin( rad );
zrot[4] = (float)cos( rad );
Matrix3x3Multiply( m, zrot, tmpmat );
Matrix3x3Multiply( tmpmat, im, rot );
dst[0] = rot[0] * point[0] + rot[1] * point[1] + rot[2] * point[2];
dst[1] = rot[3] * point[0] + rot[4] * point[1] + rot[5] * point[2];
dst[2] = rot[6] * point[0] + rot[7] * point[1] + rot[8] * point[2];
}
float *faceNormal(float *vectA, float *vectB, float *vectC)
{
float *result;
float *vectD;
float *vectE;
vectD = vectorSubtract(vectB, vectA, 3);
vectE = vectorSubtract(vectC, vectA, 3);
result = vectorCrossProduct(vectD, vectE);
delete []vectD;
delete []vectE;
return result;
}
//Correct the drift or the gyroscope to get a more accurate result
void AnglesDriftCorrection(float AccelerationVector[3])
{
double scaledIntegratorVector[3];
double accelerationMagnitude;
double accelerationWeight;
double integratorMagnitude;
double AccelerationVectorInvertedDystem[3];//We call here Inverted System the Device coordinate system with Dx = Dy and Dy = Dx( the axis are exchanged)
double errorRollPitch[3];
/******We calculate a vector proportionalVector and integratorVector to add to the gyroscopeVector to cancel the drift. Those two vectors are calculated with the accelerometer vector. It doesn't cancel the drift for the yaw angle. *******/
/****** Calculate the magnitude of the accelerometer vector***********/
accelerationMagnitude = sqrt(AccelerationVector[0]*AccelerationVector[0] + AccelerationVector[1]*AccelerationVector[1] + AccelerationVector[2]*AccelerationVector[2]);
accelerationMagnitude = accelerationMagnitude / GRAVITY; // We know have value of 1 = 1g
// Dynamic weighting of accelerometer info (reliability filter)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0- , >1.5G = 0.0)
accelerationWeight = constrain(1 - 2*abs(1 - accelerationMagnitude),0,1);
/****We make sure that the acceleration vector has the same system as the one we use in the algorithm *******/
AccelerationVectorInvertedDystem[0] = AccelerationVector[1];
AccelerationVectorInvertedDystem[1] = AccelerationVector[0];
AccelerationVectorInvertedDystem[2] = AccelerationVector[2];
/*****We calculate the weights using the fact that 1g = 101********/
vectorScale(AccelerationVectorInvertedDystem,AccelerationVectorInvertedDystem,101/9.81);
/******We calculate our two vectors proportionalVector and integratorVector********/
vectorCrossProduct(errorRollPitch,AccelerationVectorInvertedDystem,dCMMatrix[2]); //adjust the ground of reference
vectorScale(proportionalVector,errorRollPitch,kpRollpitch*accelerationWeight);
vectorScale(scaledIntegratorVector,errorRollPitch,kiRollpitch*accelerationWeight);
vectorAddition(integratorVector,integratorVector,scaledIntegratorVector);
// Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
integratorMagnitude = sqrt(vectorDotProduct(integratorVector,integratorVector));
if (integratorMagnitude > ToRad(300)) {
vectorScale(integratorVector,integratorVector,0.5f*ToRad(300)/integratorMagnitude);
}
}
void gluLookAt( vec3_t vEye, vec3_t vLookat, vec3_t vUp ,matrix_t fModelView)
{
vec3_t vN,vU,vV;
// determine the new n
vectorSubtract(vEye,vLookat,vN);
// determine the new u by crossing with the up vector
vectorCrossProduct(vUp, vN, vU) ;
// normalize both the u and n vectors
normalize(vU) ;
normalize(vN);
// determine v by crossing n and u
vectorCrossProduct(vN,vU,vV);
// create a model view matrix
fModelView[0] = vU[0]; fModelView[4] = vU[1]; fModelView[8] = vU[2]; fModelView[12] = - DotProduct(vEye,vU);
fModelView[1] = vV[0]; fModelView[5] = vV[1]; fModelView[9] = vV[2]; fModelView[13] = - DotProduct(vEye,vV);
fModelView[2] = vN[0]; fModelView[6] = vN[1]; fModelView[10]= vN[2]; fModelView[14]= - DotProduct(vEye,vN);
fModelView[3]= 0.0f; fModelView[7]= 0.0f; fModelView[11]= 0.0f; fModelView[15]= 1.0f;
}
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]);
}
/*----------------------------------------------------------------------------*/
static MATRIX buildRMatrix(MATRIX UB, MATRIX planeNormal,
tasQEPosition qe, int *errorCode){
MATRIX U1V, U2V, TV, TVINV, M;
*errorCode = 1;
U1V = tasReflectionToQC(qe,UB);
if(U1V == NULL){
*errorCode = UBNOMEMORY;
return NULL;
}
normalizeVector(U1V);
U2V = vectorCrossProduct(planeNormal,U1V);
if(U2V == NULL){
killVector(U1V);
*errorCode = UBNOMEMORY;
return NULL;
}
if(vectorLength(U2V) < .0001){
*errorCode = BADUBORQ;
killVector(U1V);
killVector(U2V);
return NULL;
}
TV = buildTVMatrix(U1V,U2V);
if(TV == NULL){
killVector(U1V);
killVector(U2V);
*errorCode = UBNOMEMORY;
return NULL;
}
TVINV = mat_inv(TV);
if(TVINV == NULL){
*errorCode = BADUBORQ;
}
killVector(U1V);
killVector(U2V);
mat_free(TV);
return TVINV;
}
/*-------------------------------------------------------------------*/
MATRIX calcPlaneNormal(tasReflection r1, tasReflection r2){
MATRIX u1 = NULL, u2 = NULL, planeNormal = NULL;
int i;
u1 = calcTasUVectorFromAngles(r1);
u2 = calcTasUVectorFromAngles(r2);
if(u1 != NULL && u2 != NULL){
planeNormal = vectorCrossProduct(u1,u2);
/*
The plane normal has to point to the stars and not to the earth
core in order for the algorithm to work.
*/
if(planeNormal[2][0] < .0){
for(i = 0; i < 3; i++){
planeNormal[i][0] = -1.*planeNormal[i][0];
}
}
return planeNormal;
} else {
return NULL;
}
}
void SkeletonState::renderLimb(SkeletonPoint& pt1, SkeletonPoint& pt2)
{
if(pt1.confidence <= 0.5 || pt2.confidence <= 0.5)
return;
float a[3] = {pt1.x, pt1.y, pt1.z};
float b[3] = {pt2.x, pt2.y, pt2.z};
// vector formed by the two joints
float c[3];
vectorSubtraction(a, b, c);
// glu cylinder vector
float z[3] = {0,0,1};
// r is axis of rotation about z
float r[3];
vectorCrossProduct(z, c, r);
// get angle of rotation in degrees
float angle = 180./M_PI * acos((vectorDotProduct(z, c)/vectorMagnitude(c)));
glPushMatrix();
// translate to second joint
glTranslatef(pt2.x, pt2.y, pt2.z);
glRotatef(angle, r[0], r[1], r[2]);
// set up quadric object
GLUquadricObj* quadObj = gluNewQuadric();
gluCylinder(quadObj, 10, 10, vectorMagnitude(c), 10, 10);
glPopMatrix();
// delete used quadric
gluDeleteQuadric(quadObj);
}
/*-----------------------------------------------------------------------------*/
static MATRIX buildTVMatrix(MATRIX U1V, MATRIX U2V){
MATRIX T, T3V;
int i;
normalizeVector(U2V);
T3V = vectorCrossProduct(U1V,U2V);
normalizeVector(T3V);
if(T3V == NULL){
return NULL;
}
T = mat_creat(3,3,ZERO_MATRIX);
if(T == NULL){
killVector(T3V);
return NULL;
}
for(i = 0; i < 3; i++){
T[i][0] = U1V[i][0];
T[i][1] = U2V[i][0];
T[i][2] = T3V[i][0];
}
killVector(T3V);
return T;
}
// Returns (b-a) x (c-a)
inline C3DTVector vectorCrossProductTri(const C3DTVector a, const C3DTVector b, const C3DTVector c) {
return vectorCrossProduct(vectorSubtract(b,a), vectorSubtract(c,a));
}
// Normal of 2 vectors (0 centered)
inline C3DTVector vectorNormal(const C3DTVector a, const C3DTVector b)
{
return vectorNormalize(vectorCrossProduct(a, b));
}
//Normalize thed DCM matrix to caculate the orientation
void AnglesNormalize()
{
double error=0;
double temporaryMatrix[3][3];
double vectorNorm=0;
bool problem=false;
/*******We want to make sure that the rows of our DCM Matrix are orthogonal. If not, we make them orthogonal.*******/
error= -vectorDotProduct(dCMMatrix[0],dCMMatrix[1])*.5f; //eq.19
vectorScale(temporaryMatrix[0], dCMMatrix[1], error); //eq.19
vectorScale(temporaryMatrix[1], dCMMatrix[0], error); //eq.19
vectorAddition(temporaryMatrix[0], temporaryMatrix[0], dCMMatrix[0]);//eq.19
vectorAddition(temporaryMatrix[1], temporaryMatrix[1], dCMMatrix[1]);//eq.19
vectorCrossProduct(temporaryMatrix[2],temporaryMatrix[0],temporaryMatrix[1]); //eq.20
/******We make sure that the norm of our vector is 1*******/
vectorNorm= vectorDotProduct(temporaryMatrix[0],temporaryMatrix[0]);
if (vectorNorm < 1.5625f && vectorNorm > 0.64f) {
vectorNorm= .5f * (3-vectorNorm); //eq.21
} else if (vectorNorm < 100.0f && vectorNorm > 0.01f) {
vectorNorm= 1. / sqrt(vectorNorm);
} else {
problem = true;
}
vectorScale(dCMMatrix[0], temporaryMatrix[0], vectorNorm);
vectorNorm= vectorDotProduct(temporaryMatrix[1],temporaryMatrix[1]);
if (vectorNorm < 1.5625f && vectorNorm > 0.64f) {
vectorNorm= .5f * (3-vectorNorm); //eq.21
} else if (vectorNorm < 100.0f && vectorNorm > 0.01f) {
vectorNorm= 1. / sqrt(vectorNorm);
} else {
problem = true;
}
vectorScale(dCMMatrix[1], temporaryMatrix[1], vectorNorm);
vectorNorm= vectorDotProduct(temporaryMatrix[2],temporaryMatrix[2]);
if (vectorNorm < 1.5625f && vectorNorm > 0.64f) {
vectorNorm= .5f * (3-vectorNorm); //eq.21
} else if (vectorNorm < 100.0f && vectorNorm > 0.01f) {
vectorNorm= 1. / sqrt(vectorNorm);
} else {
problem = true;
}
vectorScale(dCMMatrix[2], temporaryMatrix[2], vectorNorm);
/******If we can't renormalize ou matrix, then we reset it.*******/
if (problem) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
dCMMatrix[0][0]= 1.0f;
dCMMatrix[0][1]= 0.0f;
dCMMatrix[0][2]= 0.0f;
dCMMatrix[1][0]= 0.0f;
dCMMatrix[1][1]= 1.0f;
dCMMatrix[1][2]= 0.0f;
dCMMatrix[2][0]= 0.0f;
dCMMatrix[2][1]= 0.0f;
dCMMatrix[2][2]= 1.0f;
problem = false;
}
}
void MD5_GenerateLightingInfo (md5_mesh_t* mesh)
{
int verticesCounter;
int weightCounter;
//User for tangent space generation
vec3_t v1,v2,normal;
vec3_t* normalAccumulator;
vec3_t* normalWeightAccumulator;
vec3_t tangent;
float coef;
vec3_t jointSpaceTangent;
vec2_t st1,st2;
vec3_t* tangentAccumulator;
vec3_t* tangentWeightAccumulator;
vertex_t* currentVertex = NULL;
md5_vertex_t* md5Vertex;
int facesCounter;
md5_weight_t* weight;
md5_bone_t* bone;
md5_triangle_t* currentFace;
vec3_t jointSpaceNormal;
normalAccumulator = calloc(mesh->numVertices, sizeof(vec3_t));
normalWeightAccumulator = calloc(mesh->numWeights, sizeof(vec3_t));
tangentAccumulator = calloc(mesh->numVertices, sizeof(vec3_t));
tangentWeightAccumulator= calloc(mesh->numWeights, sizeof(vec3_t));
//printf("\nGenerating normal and tangents.\n");
//Generate the normal and tangent per face
currentFace = mesh->triangles;
for(facesCounter = 0; facesCounter < mesh->numTriangles ; facesCounter++,currentFace++)
{
// Normal part
vectorSubtract(mesh->vertexArray[currentFace->index[2]].pos , mesh->vertexArray[currentFace->index[0]].pos, v1);
vectorSubtract(mesh->vertexArray[currentFace->index[1]].pos , mesh->vertexArray[currentFace->index[0]].pos, v2);
vectorCrossProduct(v2,v1,normal);
normalize(normal);
vectorAdd(normalAccumulator[currentFace->index[0]],normal,normalAccumulator[currentFace->index[0]]);
vectorAdd(normalAccumulator[currentFace->index[1]],normal,normalAccumulator[currentFace->index[1]]);
vectorAdd(normalAccumulator[currentFace->index[2]],normal,normalAccumulator[currentFace->index[2]]);
// The following part is from "Mathematic for 3D programming" by Eric Lengyel
// Tangent part
vector2Subtract(mesh->vertexArray[currentFace->index[2]].text,mesh->vertexArray[currentFace->index[0]].text,st1);
vector2Subtract(mesh->vertexArray[currentFace->index[1]].text,mesh->vertexArray[currentFace->index[0]].text,st2);
vector2Scale(st1,1/(float)32767,st1);
vector2Scale(st2,1/(float)32767,st2);
if (st1[0] == 0.0f && st2[0] == 0.0f)
{
st1[0] = 0.1f ;
st2[0] = 0.1f;
}
if (st1[1] == 0.0f && st2[1] == 0.0f)
{
st1[1] = 0.1f ;
st2[1] = 0.1f;
}
coef = 1/ (st1[0] * st2[1] - st2[0] * st1[1]);
tangent[0] = coef * (v1[0] * st2[1] + v2[0] * -st1[1]);
tangent[1] = coef * (v1[1] * st2[1] + v2[1] * -st1[1]);
tangent[2] = coef * (v1[2] * st2[1] + v2[2] * -st1[1]);
normalize(tangent);
vectorAdd(tangentAccumulator[currentFace->index[0]],tangent,tangentAccumulator[currentFace->index[0]]);
vectorAdd(tangentAccumulator[currentFace->index[1]],tangent,tangentAccumulator[currentFace->index[1]]);
vectorAdd(tangentAccumulator[currentFace->index[2]],tangent,tangentAccumulator[currentFace->index[2]]);
}
//Normalize accumulated normal and tangent
for(verticesCounter=0 ; verticesCounter < mesh->numVertices ; verticesCounter++,currentVertex++)
{
normalize(normalAccumulator[verticesCounter]);
// printf("normalized accumulated normal [%d][%.2f,%.2f,%.2f]\n",verticesCounter,normalAccumulator[verticesCounter][0],normalAccumulator[verticesCounter][1],normalAccumulator[verticesCounter][2]);
normalize(tangentAccumulator[verticesCounter]);
// printf("normalized accumulated tangent [%d][%.2f,%.2f,%.2f]\n",verticesCounter,tangentAccumulator[verticesCounter][0],tangentAccumulator[verticesCounter][1],tangentAccumulator[verticesCounter][2]);
}
//Now we have all the normal for this model, but need to transform them in bone space for re-usage
// Translating the normal orientation from object to joint space and Store normals inside weights,
md5Vertex = mesh->vertices;
currentVertex = mesh->vertexArray;
for(verticesCounter=0 ; verticesCounter < mesh->numVertices ; verticesCounter++,md5Vertex++)
{
for (weightCounter = 0; weightCounter < md5Vertex->count; weightCounter++)
{
weight = &mesh->weights[md5Vertex->start + weightCounter];
bone = &mesh->bones[weight->boneId];
multiplyByInvertQuaternion(normalAccumulator[verticesCounter],bone->orientation,jointSpaceNormal);
vectorAdd(normalWeightAccumulator[md5Vertex->start + weightCounter],jointSpaceNormal,normalWeightAccumulator[md5Vertex->start + weightCounter]);
multiplyByInvertQuaternion(tangentAccumulator[verticesCounter],bone->orientation,jointSpaceTangent);
vectorAdd(tangentWeightAccumulator[md5Vertex->start + weightCounter],jointSpaceTangent,tangentWeightAccumulator[md5Vertex->start + weightCounter]);
}
}
weight = mesh->weights;
for (weightCounter = 0; weightCounter < mesh->numWeights; weightCounter++,weight++)
{
normalize(normalWeightAccumulator[weightCounter]);
vectorScale(normalWeightAccumulator[weightCounter],32767,weight->boneSpaceNormal);
normalize(tangentWeightAccumulator[weightCounter]);
vectorScale(tangentWeightAccumulator[weightCounter],32767,weight->boneSpaceTangent);
}
free(normalAccumulator);
free(normalWeightAccumulator);
free(tangentAccumulator);
free(tangentWeightAccumulator);
}
void GenerateLightingInfo (const md5_mesh_t *mesh, md5_joint_t *skeleton)
{
int verticesCounter;
int weightCounter;
//User for tangent space generation
vec3_t v1,v2,normal;
vec3_t* normalAccumulator;
vec3_t* normalWeightAccumulator;
#ifdef TANGENT_ENABLED
vec3_t tangent;
float coef;
vec3_t jointSpaceTangent;
vec2_t st1,st2;
vec3_t* tangentAccumulator;
vec3_t* tangentWeightAccumulator;
#endif
vertex_t* currentVertex = NULL;
md5_vertex_t* md5Vertex;
int facesCounter;
md5_weight_t* weight;
md5_joint_t* joint;
md5_triangle_t* currentFace;
vec3_t jointSpaceNormal;
normalAccumulator = calloc(mesh->num_verts, sizeof(vec3_t));
normalWeightAccumulator = calloc(mesh->num_weights, sizeof(vec3_t));
#ifdef TANGENT_ENABLED
tangentAccumulator = calloc(mesh->num_verts, sizeof(vec3_t));
tangentWeightAccumulator = calloc(mesh->num_weights, sizeof(vec3_t));
#endif
//Set all textures coordinate once for all.
currentVertex = mesh->vertexArray;
for(verticesCounter=0 ; verticesCounter < mesh->num_verts ; verticesCounter++,currentVertex++)
{
currentVertex->text[0] = mesh->vertices[verticesCounter].st[0];
currentVertex->text[1] = mesh->vertices[verticesCounter].st[1];
}
currentFace = mesh->triangles;
for(facesCounter = 0; facesCounter < mesh->num_tris ; facesCounter++,currentFace++)
{
// Normal part
vectorSubtract(mesh->vertexArray[currentFace->index[2]].pos , mesh->vertexArray[currentFace->index[0]].pos, v1);
vectorSubtract(mesh->vertexArray[currentFace->index[1]].pos , mesh->vertexArray[currentFace->index[0]].pos, v2);
vectorCrossProduct(v2,v1,normal);
vectorAdd(normalAccumulator[currentFace->index[0]],normal,normalAccumulator[currentFace->index[0]]);
vectorAdd(normalAccumulator[currentFace->index[1]],normal,normalAccumulator[currentFace->index[1]]);
vectorAdd(normalAccumulator[currentFace->index[2]],normal,normalAccumulator[currentFace->index[2]]);
// The following part is from "Mathematic for 3D programming" by Eric Lengyel
// Tangent part
#ifdef TANGENT_ENABLED
vector2Subtract(mesh->vertexArray[currentFace->index[2]].text,mesh->vertexArray[currentFace->index[0]].text,st1);
vector2Subtract(mesh->vertexArray[currentFace->index[1]].text,mesh->vertexArray[currentFace->index[0]].text,st2);
coef = 1/ (st1[0] * st2[1] - st2[0] * st1[1]);
tangent[0] = coef * (v1[0] * st2[1] + v2[0] * -st1[1]);
tangent[1] = coef * (v1[1] * st2[1] + v2[1] * -st1[1]);
tangent[2] = coef * (v1[2] * st2[1] + v2[2] * -st1[1]);
vectorAdd(tangentAccumulator[currentFace->index[0]],tangent,tangentAccumulator[currentFace->index[0]]);
vectorAdd(tangentAccumulator[currentFace->index[1]],tangent,tangentAccumulator[currentFace->index[1]]);
vectorAdd(tangentAccumulator[currentFace->index[2]],tangent,tangentAccumulator[currentFace->index[2]]);
#endif
}
for(verticesCounter=0 ; verticesCounter < mesh->num_verts ; verticesCounter++,currentVertex++)
{
normalize(normalAccumulator[verticesCounter]);
#ifdef TANGENT_ENABLED
normalize(tangentAccumulator[verticesCounter]);
#endif
}
//Now we have all the normal for this model, but need to transform them in bone space for re-usage
// Translating the normal orientation from object to joint space and Store normals inside weights,
md5Vertex = mesh->vertices;
currentVertex = mesh->vertexArray;
for(verticesCounter=0 ; verticesCounter < mesh->num_verts ; verticesCounter++,md5Vertex++)
{
for (weightCounter = 0; weightCounter < md5Vertex->count; weightCounter++)
{
weight = &mesh->weights[md5Vertex->start + weightCounter];
joint = &skeleton[weight->joint];
multiplyByInvertQuaternion(normalAccumulator[verticesCounter],joint->orient,jointSpaceNormal);
vectorAdd(normalWeightAccumulator[md5Vertex->start + weightCounter],jointSpaceNormal,normalWeightAccumulator[md5Vertex->start + weightCounter]);
#ifdef TANGENT_ENABLED
multiplyByInvertQuaternion(tangentAccumulator[verticesCounter],joint->orient,jointSpaceTangent);
vectorAdd(tangentWeightAccumulator[md5Vertex->start + weightCounter],jointSpaceTangent,tangentWeightAccumulator[md5Vertex->start + weightCounter]);
#endif
}
}
weight = mesh->weights;
for (weightCounter = 0; weightCounter < mesh->num_weights; weightCounter++,weight++)
{
normalize(normalWeightAccumulator[weightCounter]);
vectorScale(normalWeightAccumulator[weightCounter],32767,weight->normal);
#ifdef TANGENT_ENABLED
normalize(tangentWeightAccumulator[weightCounter]);
vectorScale(tangentWeightAccumulator[weightCounter],32767,weight->tangent);
#endif
}
free(normalAccumulator);
free(normalWeightAccumulator);
#ifdef TANGENT_ENABLED
free(tangentAccumulator);
free(tangentWeightAccumulator);
#endif
}
Vector calculateLorenzForce(double q, Vector E, Vector v, Vector B) {
return vectorMultiply(vectorSum(E, vectorCrossProduct(v, B)), q);
}