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
}
/* 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;
}
/* 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(¤tLight.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;
}
//------------------------------------------------------------------------------
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];
}
}
//------------------------------------------------------------------------------
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;
}
//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;
}
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;
}
//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]);
}
/* 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;
}
//------------------------------------------------------------------------------
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];
}
}
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);
}
// Returns the cos() of the angle between 2 vectors
inline float vectorAngleCos(const C3DTVector a, const C3DTVector b)
{
return vectorDotProduct(vectorNormalize(a), vectorNormalize(b));
}
double vectorLength(vector self) {
return sqrt(vectorDotProduct(self,self));
}
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;
}
v_t vectorProjectScalar(xy_t p1, xy_t p2)
{
return vectorDotProduct(p1, vectorUnit(p2));
}
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);
}
//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;
}
}
