void addTriangle(int v1, int v2, int v3) {
triangles3D.push_back(Triangle(vertices_local[v1-1], vertices_local[v2-1], vertices_local[v3-1]));
triangles2D.push_back(Triangle(vertices2D[v1-1], vertices2D[v2-1], vertices2D[v3-1]));
//update vertices_local's normal
vertices_local[v1-1].normal = vectorAddition(vertices_local[v1-1].normal, triangles3D.back().normal);
vertices_local[v2-1].normal = vectorAddition(vertices_local[v2-1].normal, triangles3D.back().normal);
vertices_local[v3-1].normal = vectorAddition(vertices_local[v3-1].normal, triangles3D.back().normal);
}
// Update the dCM matrix to caculate the orientation
void AnglesMatrixUpdate(float gyroscopeVector[3], int gyroscopeTimestamp)
{
double period;
if(timestampPreviousCall==0)
period = 0;
else
period = (gyroscopeTimestamp - timestampPreviousCall); //time in seconds between the two last measures we got
double temporaryVector[3]= {0,0,0};
double updateMatrix[3][3]; //temporary matrix we use to update the dCMMatrix
double temporaryMatrix[3][3];
double gyroscopeVectorInvertedSystem[3]; //We call here Inverted System the Device coordinate system with Dx = Dy and Dy = Dx( the axis are exchanged)
/****** We have to Exchange the x and y value of our vector to correspond with the system used by the algorithm *******/
gyroscopeVectorInvertedSystem[0] = gyroscopeVector[1];
gyroscopeVectorInvertedSystem[1] = gyroscopeVector[0];
gyroscopeVectorInvertedSystem[2] = gyroscopeVector[2];
/******Calculate the GyroscopeVectorCorrected which is the gyroscope vector measure plus some vectors we get with the drift correction ( corresponding to the PID of the algorithm ********/
vectorAddition(temporaryVector, gyroscopeVectorInvertedSystem, integratorVector); //adding proportional term
vectorAddition(gyroscopeVectorCorrected, temporaryVector, proportionalVector); //adding Integrator term
/******We calculate the changement that the new measurement will imply*******/
updateMatrix[0][0]=0;
updateMatrix[0][1]=-period*gyroscopeVectorCorrected[2];
updateMatrix[0][2]=period*gyroscopeVectorCorrected[1];
updateMatrix[1][0]=period*gyroscopeVectorCorrected[2];
updateMatrix[1][1]=0;
updateMatrix[1][2]=-period*gyroscopeVectorCorrected[0];
updateMatrix[2][0]=-period*gyroscopeVectorCorrected[1];
updateMatrix[2][1]=period*gyroscopeVectorCorrected[0];
updateMatrix[2][2]=0;
/******We update the DCM Matrix *******/
matrixMultiply(temporaryMatrix,dCMMatrix, updateMatrix);
for(int x=0; x<3; x++) //Matrix Addition (update)
{
for(int y=0; y<3; y++)
{
dCMMatrix[x][y]+=temporaryMatrix[x][y];
}
}
timestampPreviousCall = gyroscopeTimestamp;
}
Line *createLine(vector camera, png_uint_32 x, png_uint_32 y) {
Line *retval = malloc(sizeof(Line));
retval->start = camera;
vector planePoint = vectorCreate(-8.0 + 16.0 * ((double)x / (double)WIDTH), 0.0, 10.0 - 10.0 * ((double)y / (double)HEIGHT));
vector dir = vectorSubtraction(planePoint, camera);
retval->end = vectorAddition(camera, vectorMultiply(dir,50));
return retval;
}
//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 lightCalculation(){
float rayCalculation(float levelDetail){
/* Default closest interection to 3000*/
float closestIntersection = 3000;
int currentSphere = -1;
int i;
for(i = 0; i < numberOfSpheres; i++){
//if Ray and Sphere intersect
if((intersectRaySphere(&ray, &spheres[i], &closestIntersection)) == 1){
currentSphere = i;
}
}
//if not intersection found return
if(currentSphere == -1){
return levelDetail;
}
//Finding the scaler value of the closestInterestion and the ray directions
Vector scaled = vectorScalar(closestIntersection, &ray.direction);
//Starting at the new location just found
Vector newStart = vectorAddition(&ray.start, &scaled);
//Find the normal for this new vector at the point of intersection
Vector normal = vectorSubtraction(&newStart, &spheres[currentSphere].pos);
float temp = vectorDotProduct(&normal, &normal);
//if there's no dot product of the normal vector return
if(temp == 0){
return levelDetail;
}
temp = (1.0 / sqrtf(temp));
//Setting the noram vector
normal = vectorScalar(temp, &normal);
//calucting the lights and shadows
levelDetail = lightCalculation(levelDetail, currentSphere, &newStart, &normal);
// The reflected ray start and direction
ray.start = newStart;
float reflect = 2 * vectorDotProduct(&ray.direction, &normal);
Vector tmp = vectorScalar(reflect, &normal);
ray.direction = vectorSubtraction(&ray.direction, &tmp);
return levelDetail;
}
void testVectorAlgebra(){
double v[]={4,11,8,10};
double vLength=0;
double v1[]={3,2,1,-2};
double v2[]={2,-1,4,1};
printf("\nMatrix\n");
vLength=vectorLength(v, 4);
printf("vector length: %f\n",vLength);
printf("vector addition\n");
vectorAddition(v1, v2, 4);
printVector(v1,4);
}
vector rayTrace(Line *ray, Primitive **primitives, Light **lights, float r_idx) {
Intersection *bestIntersection = NULL;
vector retval = vectorCreate(0.0, 0.0, 0.0);
for (int p = 0; p < 10; p++) {
if (!primitives[p])
break;
Intersection *thisIntersection = primitiveIntersect(primitives[p], ray);
if (!bestIntersection) {
bestIntersection = thisIntersection;
continue;
}
if (thisIntersection && thisIntersection->distance < bestIntersection->distance) {
free(bestIntersection);
bestIntersection = thisIntersection;
}
}
if (bestIntersection) {
Primitive *primitive = bestIntersection->primitive;
Material *material = primitive->material;
vector mcolor = vectorMultiply(material->color, 0.0);
vector N = primitiveNormal(primitive, bestIntersection);
vector V = vectorUnit(vectorSubtraction(ray->end, ray->start));
for (int l = 0; l < 2; l++) {
Light *light = lights[l];
vector lvec = vectorSubtraction(light->location, bestIntersection->intersectionPoint);
vector pcolor = material->color;
vector lcolor = light->color;
Intersection *linter = NULL;
Line *lightRay = makeLine(bestIntersection->intersectionPoint, light->location);
float shade = 0.0;
for (int p = 0; p < 3; p++) {
if (primitives[p] == primitive)
continue;
if ((linter = primitiveIntersect(primitives[p], lightRay)))
shade += 1.0 - linter->primitive->material->transparency;
}
free(lightRay);
if (shade < 1.0) {
if (material->specular > 0.0) {
float sintensity = 0.0;
vector L = vectorUnit(lvec);
vector R = vectorSubtraction(L, vectorMultiply(N, 2 * vectorDotProduct(L,N)));
float dot = vectorDotProduct(V, R);
if (dot > 0.0) {
sintensity = pow(dot, 20) * material->specular * light->intensity * (1.0 - shade);
}
if (sintensity > 0.0) {
mcolor = vectorAddition(mcolor, vectorMultiply(lcolor, sintensity));
}
}
if (material->diffuse > 0.0) {
float dintensity = material->diffuse * vectorDotProduct(vectorUnit(lvec), primitive->normal(bestIntersection->primitive, bestIntersection)) * light->intensity * (1.0 - shade);
if (dintensity > 0.0) {
mcolor = vectorAddition(mcolor, vectorMultiply(vectorCProduct(pcolor, lcolor), dintensity));
}
}
}
free(linter);
}
if (material->reflection > 0.0) {
vector R = vectorUnit(vectorSubtraction(V, vectorMultiply(N, 2 * vectorDotProduct(V,N))));
Line *rline = makeLine(vectorAddition(bestIntersection->intersectionPoint, vectorMultiply(R, EPS)), vectorAddition(bestIntersection->intersectionPoint, vectorMultiply(R, 30)));
vector rcolor = rayTrace(rline, primitives, lights, r_idx);
mcolor = vectorAddition(vectorMultiply(mcolor, 1.0 - material->reflection), vectorMultiply(rcolor, material->reflection));
free(rline);
}
if (material->transparency > 0) {
float refraction = material->refraction;
float n = r_idx / refraction;
vector Nr = vectorMultiply(N, bestIntersection->direction);
float cosI = - vectorDotProduct(Nr, V);
float cosT2 = 1.0 - n * n * (1.0 - cosI * cosI);
if (cosT2 > 0.0) {
vector T = vectorAddition(vectorMultiply(V, n), vectorMultiply(Nr, n * cosI - sqrt(cosT2)));
Line *rline = makeLine(vectorAddition(bestIntersection->intersectionPoint, vectorMultiply(T, EPS)), vectorAddition(bestIntersection->intersectionPoint, vectorMultiply(T, 30)));
vector rfcol = rayTrace(rline, primitives, lights, r_idx);
mcolor = vectorAddition(vectorMultiply(mcolor, 1.0 - material->transparency), vectorMultiply(rfcol, material->transparency));
free(rline);
}
}
retval = mcolor;
free(bestIntersection);
}
return retval;
}
void KinematicMotion::move(double *coordinates) const {
// rotate first, then translate: x=R*x + T. (Remark: R*(x+T) is wrong)
matrixVectorProduct(rotationMatrix, coordinates);
vectorAddition(coordinates, translationVector);
}
//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;
}
}
