int Plane::findIntersectPoint(Ray& aRay, GzCoord aPoint){

	GzCoord w0, temp;

	if(vectorZero(normal)) return -1; //triangle degenerate to a point

	vectorConstruct(vertexList[0], aRay.origin, w0);
	float a = -vectorDotProduct(normal, w0);
	float b = vectorDotProduct(normal, aRay.direction);

	if(fabs(b) < SMALL_NUM){ //ray is parallel to triangle
		if(a == 0){
			return 2; //ray lies in triangle plane
		}else{
			return 0; //ray disjoint from triangle plane
		}
	}

	float r = a/b;
	if(r < 0.0) return 0; //ray goes away from triangle

	vectorScale(r, aRay.direction, temp);
	vectorAdd(aRay.origin, temp, aPoint);

	return 1; //one intersect point 
}
Example #2
0
/* Check if the ray and sphere intersect */
int intersectRaySpherePartB(Ray *ray, Sphere *sphere){
    
    float A, B, C, root;
    
    /* The vector dot product of the direction */
    A = vectorDotProduct(&ray->direction, &ray->direction);
    
    /* The vector distance from the start of
       the ray and the position of the circle. */
    Vector distance = vectorSubtraction(&ray->start, &sphere->pos);
    
    /* Postiion - c */
    B = 2 * vectorDotProduct(&ray->direction, &distance);
    
    /* (postion - c)^2 - r^2 */
    C = vectorDotProduct(&distance, &distance) - (sphere->radius * sphere->radius);
    
    /* Solving the discriminant */
    root = B * B - 4 * A * C;
    
    /* If the root is negtive return false/zero */
    if(root < 0){
        return 0;
    }
    return 1;
}
Example #3
0
/* Calculating the lights and shadows*/
float lightCalculation(float levelDetail, int currentSphere, Vector *newStart, Vector *normal){

     // Determine  the current material
     int getMaterialIndexValue = (spheres + currentSphere)->material;
     Material currentMaterial = materials[getMaterialIndexValue];
     
     //Find the value of the light at the current position
     int j;
    
     for( j = 0; j < numberOfLights; j++){
     
         Light currentLight = lights[j];
         Vector distance = vectorSubtraction(&currentLight.pos, newStart);
     
         //If the dot product of the normal and the distance is less than or equal to zero
         //coutinue
         if(vectorDotProduct(normal, &distance) <= 0) {
             continue;
         }
     
         float closestIntersection;
         
         //setting the new closesInterection point
         closestIntersection = sqrtf(vectorDotProduct(&distance,&distance));
     
         if(closestIntersection <= 0) {
             continue;
         }
     
         Ray lightRay;
         lightRay.start = *newStart;
         lightRay.direction = vectorScalar((1/closestIntersection), &distance);
     
         // Calculate shadows
         int shadow = 0;
         int k;
     
         for (k = 0; k < numberOfSpheres; ++k){
     
             if (intersectRaySphere(&lightRay, &spheres[k], &closestIntersection) == 1){
                 shadow = 0;
                 break;
             }
         }
         if (shadow != 1){
     
             //Calculating the lambert value
             float lambert = vectorDotProduct(&lightRay.direction, normal) * levelDetail;
             //Processing the Lambert Diffusion
             LambertDiffusion(lambert, currentLight, currentMaterial);
     
         }
        }
    levelDetail *= currentMaterial.reflection;
    
    return levelDetail;

}
Example #4
0
//------------------------------------------------------------------------------
void calc_specular(GzRender *render, GzCoord N_orig, GzColor col, bool mulByK) {
    // N is already sent here after transformation
    GzCoord N = {0.0f,0.0f,0.0f};
    normalizeVector(N_orig, N);
    
    GzCoord AccumulatedSpecResult = {0.0f, 0.0f, 0.0f};
 
    for(int i = 0; i < render->numlights; i++)
    {
        float (*ls)[3] = static_cast<float (*)[3]>(render->lights[i]);
        GzCoord ls_L_orig = {ls[0][0], ls[0][1], ls[0][2]};       
        GzCoord E = {0, 0, -1};        
        
        GzCoord ls_L = {0.0f,0.0f,0.0f};
        normalizeVector(ls_L_orig, ls_L);   
        


        if (!shouldCalcLight(N, ls_L, E, N))
            continue;
        
        
        
        float N_dot_L = vectorDotProduct(N, ls_L);
        GzCoord left = {0.0f,0.0f,0.0f};
        scalarMultiply(N, N_dot_L * 2.0f, left);
        GzCoord R_orig = {0.0f,0.0f,0.0f};
        subtractVector(left, ls_L, R_orig);
        GzCoord R = {0.0f,0.0f,0.0f};
        normalizeVector(R_orig, R);
        
        GzCoord ls_intensity = {ls[1][0], ls[1][1], ls[1][2]};
        
        
        GzCoord localResult = {0.0f,0.0f,0.0f};
        
        float RdotE = vectorDotProduct(R,E);
        if (RdotE < 0) RdotE = 0;
        if (RdotE > 1) RdotE = 1;
        
        scalarMultiply(ls_intensity, 
                       powf(RdotE, render->spec), 
                       localResult);
        
        addVectors(localResult, AccumulatedSpecResult, AccumulatedSpecResult);        
    }
    
     
    if(mulByK)
        vectorMulElementByElement(render->Ks, AccumulatedSpecResult, col);
    else
    {
        col[RED]   = AccumulatedSpecResult[RED];
        col[GREEN] = AccumulatedSpecResult[GREEN];
        col[BLUE]  = AccumulatedSpecResult[BLUE];
        
    }
}
Example #5
0
//------------------------------------------------------------------------------
bool shouldCalcLight(GzCoord N, GzCoord L, GzCoord E, GzCoord Nnew) {
    float NdotL = vectorDotProduct(N, L);
    float NdotE = vectorDotProduct(N, E);
    bool shouldCalc = (NdotL * NdotE) > 0? true: false;
    
    if(NdotL < 0 && NdotE < 0)
    {
        scalarMultiply(N, -1.0f, Nnew);
    }
    
    return shouldCalc;
}
Example #6
0
//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;
}
// If the ray intersects the sphere, return nearest distance along ray at which
// it happens. If it doesn't, return -1
int sphereIntersection(struct Sphere sphere, struct Ray ray)
{
    // The intersection algorithm assumes that the ray starts at the origin,
    // so translate the sphere first.
    sphere.position = vectorSubtract(sphere.position, ray.origin);

    int directionDotPosition = \
        vectorUnitDotProduct2(sphere.position, ray.direction);
    int sqrtTerm = fp_mult(directionDotPosition,directionDotPosition) \
                    - vectorDotProduct(sphere.position, sphere.position) \
                    + fp_mult(sphere.radius,sphere.radius);
    
    int result = -1; // Return value for no intersection

    if (sqrtTerm >= 0) // If there is an intersection (more likely 2)
    {
        sqrtTerm = fp_sqrt(sqrtTerm);
        // There are usually two solutions, for the two intersection points
        // between the ray and sphere.
        int solution1 = directionDotPosition + sqrtTerm;
        int solution2 = directionDotPosition - sqrtTerm;

        // We want the nearest non-negative (behind the ray origin) intersection
        if (solution1 >= 0)
        {
            result = solution1;
            if (solution2 >= 0 && solution2 < solution1)
                result = solution2;
        }
        else if (solution2 >= 0)
            result = solution2;
    }

    return result;
}
Example #8
0
xy_t vectorProject(xy_t p1, xy_t p2)
{
	xy_t normal = vectorUnit(p2);
	v_t scalar = vectorDotProduct(p1, normal);

	normal.x *= scalar;
	normal.y *= scalar;

	return normal;
}
bool Plane::checkPointInTriangle(GzCoord aPoint){
	GzCoord vector0, vector1, vector2;

	//compute vectors
	vectorConstruct(vertexList[0], vertexList[2], vector0);
	vectorConstruct(vertexList[0], vertexList[1], vector1);
	vectorConstruct(vertexList[0], aPoint, vector2);

	//compute dot products
	float dot00 = vectorDotProduct(vector0, vector0);
	float dot01 = vectorDotProduct(vector0, vector1);
	float dot02 = vectorDotProduct(vector0, vector2);
	float dot11 = vectorDotProduct(vector1, vector1);
	float dot12 = vectorDotProduct(vector1, vector2);

	//compute barycentric coordinates
	float invDenom = 1.0/(dot00*dot11 - dot01*dot01);
	float u = (dot11*dot02 - dot01*dot12) * invDenom;
	float v = (dot00*dot12 - dot01*dot02) * invDenom;

	//check if point is in the triangle
	return (u >= 0) && (v >= 0) && (u + v < 1.0);
}
Vector2f BouncingThing::projectOntoAxis(Vector2f axis) const {
	//normalize the axis
	float magnitude = sqrt((axis.x * axis.x) + (axis.y * axis.y));
	axis /= magnitude;

	//keep track of minimum and maximum points on the axis
	double min = vectorDotProduct(axis, m_points[0] + m_centrePos);
	double max = min;

	//loop through the points and find the min and max values for projection
	for (int i = 1; i < m_points.size(); i++) {
		double p = vectorDotProduct(axis, m_points[i] + m_centrePos);
		if (p < min) {
			min = p;
		}
		else if (p > max) {
			max = p;
		}
	}

	Vector2f proj = Vector2f(min, max);
	return proj;
}
Example #11
0
//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);
    }
    
    
}
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]);
}
Example #13
0
/* Check if the ray and sphere intersect */
int intersectRaySphere(Ray *ray, Sphere *sphere, float *closestIntersection){
    
    float A,B,C;
    int intersect = 0;
    
    /* A = direction * direction , the vector dot product of the direction */
    A = vectorDotProduct(&ray->direction, &ray->direction);
    
    /* The vector distance from the start of
     the ray and the position of the circle. */
    Vector distance = vectorSubtraction(&ray->start, &sphere->pos);
    
    /* 2*(postion - c) */
    B = 2 * vectorDotProduct(&ray->direction, &distance);
    
    /* (position - c)^2 - r^2 */
    C = vectorDotProduct(&distance, &distance) - (sphere->radius * sphere->radius);
    
    /*Solve the  Quadratic Formula */
    
    intersect = quadraticFormula(A, B, C, closestIntersection);
    
    return intersect;
}
Example #14
0
//------------------------------------------------------------------------------
void calc_diffuse(GzRender *render, GzCoord N_orig, GzColor col, bool mulByK) {
    // N is already Ncm transformed.    
    GzCoord N = {0.0f,0.0f,0.0f};
    normalizeVector(N_orig, N);
    
    GzCoord AccumulatedDiffuseResult = {0.0f, 0.0f, 0.0f};
    
    for(int i = 0; i < render->numlights; i++)
    {
        float (*ld)[3] = static_cast<float (*)[3]>(render->lights[i]);
        GzCoord ld_L_orig = {ld[0][0], ld[0][1], ld[0][2]};       
        GzCoord ld_L = {0.0f,0.0f,0.0f};         
        normalizeVector(ld_L_orig, ld_L);

        GzCoord E = {0,0,-1};
        
        if (!shouldCalcLight(N, ld_L, E, N))
            continue;
        
        
        float N_dot_L = vectorDotProduct(N, ld_L);                
        GzCoord ld_intensity = {ld[1][0], ld[1][1], ld[1][2]}; 
        
        GzCoord localResult = {0.0f,0.0f,0.0f};
        scalarMultiply(ld_intensity, N_dot_L, localResult);
        addVectors(localResult, AccumulatedDiffuseResult, AccumulatedDiffuseResult);
    }
    
    
    if(mulByK)
        vectorMulElementByElement(render->Kd, AccumulatedDiffuseResult, col);
    else
    {
        col[RED]   = AccumulatedDiffuseResult[RED];
        col[GREEN] = AccumulatedDiffuseResult[GREEN];
        col[BLUE]  = AccumulatedDiffuseResult[BLUE];
        
    }
}
Example #15
0
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);
}
Example #16
0
// Returns the cos() of the angle between 2 vectors
inline float vectorAngleCos(const C3DTVector a, const C3DTVector b)
{
    return vectorDotProduct(vectorNormalize(a), vectorNormalize(b));
}
Example #17
0
File: vector.c Project: wito/yaps
double vectorLength(vector self) {
    return sqrt(vectorDotProduct(self,self));
}
Example #18
0
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;
}
Example #19
0
v_t vectorProjectScalar(xy_t p1, xy_t p2)
{
	return vectorDotProduct(p1, vectorUnit(p2));
}
Example #20
0
bool vectorIsBetween(xy_t p, xy_t left, xy_t right)
{
	v_t leftRight = vectorDotProduct(left, right);

	return leftRight < vectorDotProduct(left, p) && leftRight < vectorDotProduct(right, p);
}
Example #21
0
//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;  
    }
    
}