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); }