void ConjugateGradient( float **A, float *b, float *x, int N, int iterations)
{
// vectors
float r[N];
float w[N];
float z[N];
// scalars
float alpha;
float beta;
float temp[N];
// initialization of residual vector: r
matrixVectorProduct( A, x, temp, N);
vectorDifference( b, temp, r, N);
// w
vectorScale( r, -1, w, N);
// z
matrixVectorProduct( A, w, z, N);
// alpha
alpha = dotProduct( r, w, N) / dotProduct( w, z, N);
// beta
beta = 0.;
// x
vectorScale( w, alpha, temp, N);
vectorSum( x, temp, x, N);
for( int i=0; i<iterations; i++){
vectorScale( z, alpha, temp, N);
vectorDifference( r, temp, r, N);
if( sqrt( dotProduct( r, r, N)) < 1e-10)
return;
// B = (r'*z)/(w'*z);
beta = dotProduct( r, z, N) / dotProduct( w, z, N);
// w = -r + B*w;
vectorScale( w, beta, w, N);
vectorDifference( w, r, w, N);
// z = A*w;
matrixVectorProduct( A, w, z, N);
// a = (r'*w)/(w'*z);
alpha = dotProduct( r, w, N) / dotProduct( w, z, N);
// x = x + a*w;
vectorScale( w, alpha, temp, N);
vectorSum( x, temp, x, N);
}
}
/**
* Prepare a mesh for drawing. Compute mesh's final vertex positions
* given a skeleton. Put the vertices in vertex arrays.
*/
void GenerateGPUVertices (md5_mesh_t *mesh, const md5_joint_t *skeleton)
{
int i, j;
const md5_weight_t *weight;
const md5_joint_t *joint ;
vec3_t tmpNormal,tmpVertex;
vec3_t normalAccumulator;
#ifdef TANGENT_ENABLED
vec3_t tmpTangent;
vec3_t tangentAccumulator;
#endif
/* Setup vertices */
vertex_t* currentVertex = mesh->vertexArray ;
for (i = 0; i < mesh->num_verts; ++i)
{
vectorClear(currentVertex->pos);
vectorClear(normalAccumulator);
#ifdef TANGENT_ENABLED
vectorClear(tangentAccumulator);
#endif
// Calculate final vertex to draw with weights
for (j = 0; j < mesh->vertices[i].count; j++)
{
weight= &mesh->weights[mesh->vertices[i].start + j];
joint = &skeleton[weight->joint];
// Calculate transformed vertex for this weight
Quat_rotatePoint (joint->orient, weight->pos, tmpVertex);
currentVertex->pos[0] += (joint->pos[0] + tmpVertex[0]) * weight->bias;
currentVertex->pos[1] += (joint->pos[1] + tmpVertex[1]) * weight->bias;
currentVertex->pos[2] += (joint->pos[2] + tmpVertex[2]) * weight->bias;
// Same thing for normal
Quat_rotateShortPoint (joint->orient, weight->normal, tmpNormal);
vectorAdd(normalAccumulator,tmpNormal,normalAccumulator);
#ifdef TANGENT_ENABLED
Quat_rotateShortPoint (joint->orient, weight->tangent, tmpTangent);
vectorAdd(tangentAccumulator,tmpTangent,tangentAccumulator);
#endif
}
//Need to normalize normal
normalize(normalAccumulator);
vectorScale(normalAccumulator,32767,currentVertex->normal);
#ifdef TANGENT_ENABLED
normalize(tangentAccumulator);
vectorScale(tangentAccumulator,32767,currentVertex->tangent);
#endif
currentVertex++;
}
}
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);
}
void orthoNormalize(int * matrix){
int delta[6];
int error = -1/2 * dot(&matrix[0], &matrix[3]);
vectorScale(&matrix[3], error, &delta[0]);
vectorScale(&matrix[0], error, &delta[3]);
vectorAdd(&matrix[0], &delta[0], &matrix[0]);
vectorAdd(&matrix[3], &delta[3], &matrix[3]);
cross(&matrix[0], &matrix[3], &matrix[6]);
mNormalize(matrix, matrix);
}
void ConjugateGradient3( float **A, float *b, float *x, int N, int iterations)
{
float r[N];
float r2[N];
float p[N];
float q[N];
//float beta[N];
//float alpha[N];
float beta;
float alpha;
float temp[N];
// initialization of residual vector: r_0
matrixVectorProduct( A, x, temp, N);
vectorDifference( b, temp, r, N);
// initialization of p_0
vectorCopy( r, p, N);
for( int i=0; i<iterations; i++){
// q_k
matrixVectorProduct( A, p, q, N);
alpha = dotProduct( r, r, N);
if(i>0){
beta = alpha / dotProduct( r2, r2, N);
vectorScale( p, beta, p, N);
vectorSum( r, p, p, N);
}
alpha /= dotProduct( p, q, N);
// next x
vectorScale( p, alpha, temp, N);
vectorSum( x, temp, x, N);
// old r
vectorCopy( r, r2, N);
// next r
vectorScale( q, -alpha, temp, N);
vectorSum( r, temp, r, N);
//float *pointer ;
}
}
//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 orthoNormalize(int * matrix){
int delta[6];
int i;
int error = -1/2 * dot(&matrix[0], &matrix[3]);
vectorScale(&matrix[3], &error, &delta[0]);
vectorScale(&matrix[0], &error, &delta[3]);
vectorAdd(&matrix[0], &delta[0], &matrix[0]);
vectorAdd(&matrix[3], &delta[3], &matrix[3]);
cross(&matrix[0], &matrix[3], &matrix[6]);
for(i = 0; i < 3; i ++){
normalize(&matrix[i * 3], &matrix[i * 3]);
}
}
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
}
Vector Biot_Savart_SingleLineSeg(LineSeg seg,Vector fieldpoint)
{
Vector result,r,Seg_Cross_r;
Vector SegMidPoint,SegVector,SegDirection;
Vector BDirection;
double SegLength,x,z,constval;
double Bmag;
SegMidPoint=linesegCenter(seg);
SegLength=linesegLength(seg);
SegVector=linesegVector(seg);
SegDirection=vectorUnitVector(SegVector);
r=linesegVector(linesegGenerate(SegMidPoint,fieldpoint));
Seg_Cross_r=vectorCross(SegVector,r);
BDirection=vectorUnitVector(Seg_Cross_r);
z=vectorMagnitude(Seg_Cross_r)/SegLength;
constval=mu_0/(4*M_PI*z);
x=vectorDot(SegDirection,r);
if(z == 0)
{
Bmag=0;
}
else
{
Bmag=constval*((SegLength-2*x)/sqrt((SegLength-2*x)*(SegLength-2*x)+4*z*z)+(SegLength+2*x)/sqrt((SegLength+2*x)*(SegLength+2*x)+4*z*z));
}
//printf("Bmag %g\n",Bmag);
result=vectorScale(BDirection,Bmag);
//printf("result.x,result.y,result.z %g,%g,%g\n",result.x,result.y,result.z);
return result;
}
float *faceCenter(float *vectA, float *vectB, float *vectC)
{
float *result;
result = vectorAdd(vectA, vectB, 3);
result = vectorUpdate(result, vectorAdd(result, vectC, 3));
result = vectorUpdate(result, vectorScale(result, 3, (float) 1 / 3));
return result;
}
void object::goFwd(float dist)
{
float *tempA;
tempA = new float[3];
vectorFromAngle(tempA, mAngle);
vectorScale(tempA, tempA, dist);
vectorAdd(mOrigin, mOrigin, tempA);
delete []tempA;
}
long colorScale(long color, float scale)
{
long result;
float *vectA;
float *vectB;
vectA = vectorFromColor(color);
vectB = vectorScale(vectA, 3, scale);
result = rgb((long) vectB[0], (long) vectB[1], (long) vectB[2]);
delete []vectA;
delete []vectB;
return result;
}
long colorRandom()
{
long result;
float *vectA;
float *vectB;
vectA = vectorRandom(3);
vectB = vectorScale(vectA, 3, 255);
result = rgb((long) vectB[0], (long) vectB[1], (long) vectB[2]);
delete []vectA;
delete []vectB;
return result;
}
/* Not really the best or most accurate way, but it works... */
double AutoExposure(scene *myScene)
{
#define ACCUMULATION_SIZE 256
double exposure = -1.0f;
double accumulationfactor = (double)max(myScene->width, myScene->height);
double projectionDistance = 0.0f;
if ((myScene->persp.type == CONIC) && myScene->persp.FOV > 0.0f && myScene->persp.FOV < 189.0f) {
projectionDistance = 0.5f * myScene->width / tanf ((double)(PIOVER180) * 0.5f * myScene->persp.FOV);
}
accumulationfactor = accumulationfactor / ACCUMULATION_SIZE;
colour mediumPoint = {0.0f, 0.0f, 0.0f};
const double mediumPointWeight = 1.0f / (ACCUMULATION_SIZE*ACCUMULATION_SIZE);
int x,y;
for (y = 0; y < ACCUMULATION_SIZE; ++y) {
for (x = 0 ; x < ACCUMULATION_SIZE; ++x) {
if (myScene->persp.type == ORTHOGONAL || projectionDistance == 0.0f) {
ray viewRay = { {(double)x*accumulationfactor, (double)y * accumulationfactor, -10000.0f}, { 0.0f, 0.0f, 1.0f}};
colour currentColor = raytrace(&viewRay, myScene);
colour tmp = colourMul(¤tColor, ¤tColor);
tmp = colourCoefMul(mediumPointWeight, &tmp);
mediumPoint = colourAdd(&mediumPoint,&tmp);
} else {
vector dir = {((double)(x)*accumulationfactor - 0.5f * myScene->width) / projectionDistance,
((double)(y)*accumulationfactor - 0.5f * myScene->height) / projectionDistance,
1.0f
};
double norm = vectorDot(&dir,&dir);
if (norm == 0.0f)
break;
dir = vectorScale(invsqrtf(norm), &dir);
ray viewRay = { {0.5f * myScene->width, 0.5f * myScene->height, 0.0f}, {dir.x, dir.y, dir.z} };
colour currentColor = raytrace(&viewRay, myScene);
colour tmp = colourMul(¤tColor, ¤tColor);
tmp = colourCoefMul(mediumPointWeight, &tmp);
mediumPoint = colourAdd(&mediumPoint,&tmp);
}
}
}
double mediumLuminance = sqrtf(0.2126f * mediumPoint.red + 0.715160f * mediumPoint.green + 0.072169f * mediumPoint.blue);
if (mediumLuminance > 0.001f) {
// put the medium luminance to an intermediate gray value
exposure = logf(0.6f) / mediumLuminance;
}
return exposure;
}
bool MeshShape::checkCollisionPeer(SphereShape *target) {
float position[3] = {0, 0, 0};
target->object->transformPoint(position, position);
object->unTransformPoint(position, position);
float normal[3];
float contactpoint[3];
float r = target->getRadius();
if (checkSphereMeshCollision(position, r, mesh, normal, contactpoint)) {
vectorScale(normal, -1);
object->transformVector(normal, normal);
object->transformPoint(contactpoint, contactpoint);
addCollision(object, target->object, normal, contactpoint);
// vectorAdd(contactnormal, normal);
return true;
}
return false;
}
bool checkEdgeMeshCollision(float *p1, float *p2, Mesh *mesh, float *normal,
float *contactpoint) {
float ray[3];
vectorSub(ray, p2, p1);
// UNUSED//float maxdist = 0;
// UNUSED//bool collision = false;
int i, j;
tracehit hits[MAXPOLYGONS];
int hitcount = 0;
for (i = 0; i < mesh->polygoncount; i++) {
class Polygon *polygon = &mesh->polygons[i];
if (tracePlane(&hits[hitcount], p1, ray, polygon)) {
hitcount++;
}
}
if (hitcount < 2)
return false;
for (i = 1; i < hitcount; i++) {
for (j = i; j > 0; j--) {
if (hits[j].t < hits[j - 1].t) {
float tempt = hits[j].t;
hits[j].t = hits[j - 1].t;
hits[j - 1].t = tempt;
class Polygon *tempp = hits[j].polygon;
hits[j].polygon = hits[j - 1].polygon;
hits[j - 1].polygon = tempp;
} else
break;
}
}
int negative = -1, positive = -1;
for (i = 0; i < hitcount; i++) {
// UNUSED//float t = hits[i].t;
class Polygon *polygon = hits[i].polygon;
float dot = vectorDot(ray, polygon->planenormal);
if (dot > 0 && positive == -1)
positive = i;
if (dot < 0)
negative = i;
if (dot < 0 && positive != -1)
return false;
}
if (negative == -1 || positive == -1)
return false;
/*for (i = 0; i < hitcount; i++){
float t = hits[i].t;
class Polygon *polygon = hits[i].polygon;
float dot = vectorDot(ray, polygon->planenormal);
printf("%f ", dot);
}
printf("\n");*/
if (hits[negative].t < 0 || hits[positive].t > 1)
return false;
Edge *edge2 = findSharingEdge(hits[negative].polygon, hits[positive].polygon);
// fflush(stdout);
float cp1[3], cp2[3];
vectorScale(cp1, ray, hits[negative].t);
vectorAdd(cp1, p1);
vectorScale(cp2, ray, hits[positive].t);
vectorAdd(cp2, p1);
if (edge2 != NULL) {
/*float ev1[3];
vectorSub(ev1, edge2->v2->position, edge2->v1->position);
vectorCross(normal, ev1, ray);
vectorScale(normal, vectorDot(normal, hits[positive].polygon->planenormal));
vectorNormalize(normal);
float at = (hits[negative].t + hits[positive].t) / 2;
vectorScale(contactpoint, ray, at);
vectorAdd(contactpoint, p1);*/
float dot1 = fabs(vectorDot(ray, hits[negative].polygon->planenormal));
float dot2 = fabs(vectorDot(ray, hits[positive].polygon->planenormal));
if (dot1 > dot2) {
// vectorScale(contactpoint, ray, hits[negative].t);
// vectorAdd(contactpoint, p1);
vectorCopy(contactpoint, cp1);
vectorCopy(normal, hits[positive].polygon->planenormal);
} else {
// vectorScale(contactpoint, ray, hits[positive].t);
// vectorAdd(contactpoint, p1);
vectorCopy(contactpoint, cp2);
vectorCopy(normal, hits[negative].polygon->planenormal);
}
} else {
Polygon *polygon = findNearestPolygon(hits[negative].polygon, cp1);
if (polygon != NULL) {
/*vectorCopy(contactpoint, cp1);
vectorAdd(contactpoint, cp2);
vectorScale(contactpoint, 0.5);*/
float at = (hits[negative].t + hits[positive].t) / 2;
vectorScale(contactpoint, ray, at);
vectorAdd(contactpoint, p1);
vectorCopy(normal, polygon->planenormal);
} else {
return false;
}
}
// shotsound->play();
return true;
}
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
}
/**
* generateBezier :
* Use least-squares method to find Bezier control points for region.
*
*/
QPointF* generateBezier(QPolygonF &points, int first, int last, double *uPrime,FitVector tHat1,FitVector tHat2)
{
int i;
FitVector A[MAXPOINTS][2]; /* Precomputed rhs for eqn */
int nPts; /* Number of pts in sub-curve */
double C[2][2]; /* Matrix C */
double X[2]; /* Matrix X */
double det_C0_C1, /* Determinants of matrices */
det_C0_X,
det_X_C1;
double alpha_l = 0, /* Alpha values, left and right */
alpha_r = 0;
FitVector tmp; /* Utility variable */
QPointF *curve;
curve = new QPointF[4];
nPts = last - first + 1;
/* Compute the A's */
for (i = 0; i < nPts; i++) {
FitVector v1, v2;
v1 = tHat1;
v2 = tHat2;
v1.scale(b1(uPrime[i]));
v2.scale(b2(uPrime[i]));
A[i][0] = v1;
A[i][1] = v2;
}
/* Create the C and X matrices */
C[0][0] = 0.0;
C[0][1] = 0.0;
C[1][0] = 0.0;
C[1][1] = 0.0;
X[0] = 0.0;
X[1] = 0.0;
for (i = 0; i < nPts; i++) {
C[0][0] += (A[i][0]).dot(A[i][0]);
C[0][1] += A[i][0].dot(A[i][1]);
C[1][0] = C[0][1];
C[1][1] += A[i][1].dot(A[i][1]);
FitVector vfirstp1(points[first+i]);
FitVector vfirst(points[first]);
FitVector vlast(points[last]);
tmp = vectorSub(vfirstp1,
vectorAdd(vectorScale(vfirst, b0(uPrime[i])),
vectorAdd(vectorScale(vfirst, b1(uPrime[i])),
vectorAdd(vectorScale(vlast, b2(uPrime[i])),
vectorScale(vlast, b3(uPrime[i])) ))));
X[0] += A[i][0].dot(tmp);
X[1] += A[i][1].dot(tmp);
}
/* Compute the determinants of C and X */
det_C0_C1 = C[0][0] * C[1][1] - C[1][0] * C[0][1];
det_C0_X = C[0][0] * X[1] - C[0][1] * X[0];
det_X_C1 = X[0] * C[1][1] - X[1] * C[0][1];
/* Finally, derive alpha values */
if (det_C0_C1 == 0.0)
det_C0_C1 = (C[0][0] * C[1][1]) * 10e-12;
if (det_C0_C1 != 0.0f) {
alpha_l = det_X_C1 / det_C0_C1;
alpha_r = det_C0_X / det_C0_C1;
}
// FIXME: else???
/* If alpha negative, use the Wu/Barsky heuristic (see text) */
/* (if alpha is 0, you get coincident control points that lead to
* divide by zero in any subsequent newtonRaphsonRootFind() call. */
if (alpha_l < 1.0e-6 || alpha_r < 1.0e-6) {
double dist = distance(points[last], points[first]) / 3.0;
curve[0] = points[first];
curve[3] = points[last];
tHat1.scale(dist);
tHat2.scale(dist);
curve[1] = tHat1 + curve[0];
curve[2] = tHat2 + curve[3];
return curve;
}
/* First and last control points of the Bezier curve are */
/* positioned exactly at the first and last data points */
/* Control points 1 and 2 are positioned an alpha distance out */
/* on the tangent vectors, left and right, respectively */
curve[0] = points[first];
curve[3] = points[last];
tHat1.scale(alpha_l);
tHat2.scale(alpha_r);
curve[1] = tHat1 + curve[0];
curve[2] = tHat2 + curve[3];
return (curve);
}
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 MD5_GenerateSkin (md5_mesh_t* mesh, md5_bone_t* bones)
{
int i, j;
md5_weight_t* weight;
md5_bone_t* bone ;
vec3_t normalAccumulator;
vec3_t tangentAccumulator;
vertex_t* currentVertex;
//printf("\nGenerating weight positions.\n");
// Generate weight position in modelSpace
weight = mesh->weights;
for (i = 0; i < mesh->numWeights; i++,weight++)
{
bone = &bones[weight->boneId];
Quat_rotatePoint (bone->orientation, weight->boneSpacePos, weight->modelSpacePos);
vectorAdd(weight->modelSpacePos,bone->position,weight->modelSpacePos);
// printf("weight[%d].pos=[%.2f,%.2f,%.2f]\n",i,bone->position[0],bone->position[1],bone->position[2]);
// printf("weight[%d].pos=[%.2f,%.2f,%.2f]\n",i,weight->modelSpacePos[0],weight->modelSpacePos[1],weight->modelSpacePos[2]);
Quat_rotateShortPoint (bone->orientation, weight->boneSpaceNormal, weight->modelSpaceNormal);
Quat_rotateShortPoint (bone->orientation, weight->boneSpaceTangent,weight->modelSpaceTangent);
}
/* Setup vertices */
currentVertex = mesh->vertexArray ;
for (i = 0; i < mesh->numVertices; ++i)
{
vectorClear(currentVertex->pos);
vectorClear(normalAccumulator);
vectorClear(tangentAccumulator);
// Calculate final vertex to draw with weights
for (j = 0; j < mesh->vertices[i].count; j++)
{
weight= &mesh->weights[mesh->vertices[i].start + j];
bone = &bones[weight->boneId];
// Calculate transformed vertex for this weight
currentVertex->pos[0] += weight->modelSpacePos[0] * weight->bias;
currentVertex->pos[1] += weight->modelSpacePos[1] * weight->bias;
currentVertex->pos[2] += weight->modelSpacePos[2] * weight->bias;
// Same thing for normal
vectorAdd(normalAccumulator,weight->modelSpaceNormal,normalAccumulator);
vectorAdd(tangentAccumulator,weight->modelSpaceTangent,tangentAccumulator);
}
// printf("currentVertex[%d].pos=[%.2f,%.2f,%.2f]\n",i,currentVertex->pos[0],currentVertex->pos[1],currentVertex->pos[2]);
//Need to normalize normal
normalize(normalAccumulator);
vectorScale(normalAccumulator,32767,currentVertex->normal);
// printf("currentVertex[%d].normal=[%hu,%hu,%hu]\n",i,currentVertex->normal[0],currentVertex->normal[1],currentVertex->normal[2]);
normalize(tangentAccumulator);
vectorScale(tangentAccumulator,32767,currentVertex->tangent);
// printf("currentVertex[%d].tangent=[%hu,%hu,%hu]\n",i,currentVertex->tangent[0],currentVertex->tangent[1],currentVertex->tangent[2]);
currentVertex++;
}
}
//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;
}
}
bool handleLink(ObjectLink *link) {
if (!link->enabled)
return false;
Object *source = link->object1;
Object *target = link->object2;
float normal[3];
float contactpoint1[3], contactpoint2[3];
source->transformPoint(contactpoint1, link->point1);
target->transformPoint(contactpoint2, link->point2);
float diff[3];
vectorSub(diff, contactpoint1, contactpoint2);
vectorNormalize(normal, diff);
float strength = vectorDot(diff, diff);
if (strength < 1.0e-5)
return false;
float sourcevelocity[3], targetvelocity[3];
float sourcecontactpoint[3], targetcontactpoint[3];
vectorSub(sourcecontactpoint, contactpoint1, source->position);
source->getVelocity(sourcevelocity, sourcecontactpoint);
vectorSub(targetcontactpoint, contactpoint2, target->position);
target->getVelocity(targetvelocity, targetcontactpoint);
float deltavelocity[3];
vectorSub(deltavelocity, sourcevelocity, targetvelocity);
float dot = vectorDot(deltavelocity, normal);
// if (fabs(dot) < EPSILON) return false;
// if (dot > -1.0e-5 && dot < 1.0e-5) return false;
// if (dot >= 0) return false;
// if (dot > -1.0e-5) return false;
float invmass1 = source->invmass;
float invmass2 = target->invmass;
float t1;
if (source->invmomentofinertia == 0) {
t1 = 0;
} else {
float v1[3];
vectorCross(v1, sourcecontactpoint, normal);
vectorScale(v1, source->invmomentofinertia);
float w1[3];
vectorCross(w1, v1, sourcecontactpoint);
t1 = vectorDot(normal, w1);
}
float t2;
if (target->invmomentofinertia == 0) {
t2 = 0;
} else {
float v1[3];
vectorCross(v1, targetcontactpoint, normal);
vectorScale(v1, target->invmomentofinertia);
float w1[3];
vectorCross(w1, v1, targetcontactpoint);
t2 = vectorDot(normal, w1);
}
float denominator = invmass1 + invmass2 + t1 + t2;
float impulsesize = (dot + strength * 100) / denominator;
// printf("%f\n", impulsesize);
float impulse[3];
vectorScale(impulse, normal, impulsesize);
target->addImpulse(impulse, targetcontactpoint);
target->calculateStateVariables();
vectorScale(impulse, -1);
source->addImpulse(impulse, sourcecontactpoint);
source->calculateStateVariables();
return true;
}
int main(int argc, char *argv[])
{
/* Scene */
scene mine;
mine.materials = NULL;
mine.spheres = NULL;
mine.triangles = NULL;
mine.lights = NULL;
myScene = &mine;
perlin noise;
myNoise = &noise;
noise_init(myNoise);
/* Threads */
thread_info *tinfo;
pthread_attr_t attr;
int t;
/* Allocate memory for pthread_create() arguments */
tinfo = calloc(NUMTHREADS, sizeof(thread_info));
if (tinfo == NULL) {
printf("Error allocating memory for pthread_create() arguments. \n");
return -1;
}
if (argc < 2) {
printf("specify scene file. \n");
return -1;
}
/* Build the scene */
if (tokenizer(argv[1], myScene) == -1) {
exit(-1);
}
/* Let's try the 3ds loader... */
if (argc == 3) {
obj_type object;
Load3DS(&object, argv[2]); /* spaceship.3ds */
int index;
printf("Polygons: %d \n", object.polygons_qty);
myScene->triangles = (triangle *)realloc(myScene->triangles, object.polygons_qty*(sizeof(triangle)));
memset(myScene->triangles,0,sizeof(triangle));
myScene->numTriangles = object.polygons_qty;
for (index=0;index<object.polygons_qty;index++) {
/* First Vertex */
myScene->triangles[index].v2.x = object.vertex[object.polygon[index].a].x+130;
myScene->triangles[index].v2.y = object.vertex[object.polygon[index].a].y+150;
myScene->triangles[index].v2.z = object.vertex[object.polygon[index].a].z+350;
myScene->triangles[index].v2 = vectorScale(2,&myScene->triangles[index].v2);
/* Second Vertex */
myScene->triangles[index].v1.x = object.vertex[object.polygon[index].b].x+130;
myScene->triangles[index].v1.y = object.vertex[object.polygon[index].b].y+150;
myScene->triangles[index].v1.z = object.vertex[object.polygon[index].b].z+350;
myScene->triangles[index].v1 = vectorScale(2,&myScene->triangles[index].v1);
/* Third Vertex */
myScene->triangles[index].v3.x = object.vertex[object.polygon[index].c].x+130;
myScene->triangles[index].v3.y = object.vertex[object.polygon[index].c].y+150;
myScene->triangles[index].v3.z = object.vertex[object.polygon[index].c].z+350;
myScene->triangles[index].v3 = vectorScale(2,&myScene->triangles[index].v3);
/* Default Material */
myScene->triangles[index].material = 3;
}
}
/* Allocate memory for the image */
if (img)
free(img);
img = (unsigned char *)malloc(3*myScene->width*myScene->height);
memset(img, 0, 3*myScene->width*myScene->height);
/* Calculate section size per thread */
sectionsize = myScene->height/NUMTHREADS;
if ((sectionsize % 2)!=0)
printf("Warning: Height/numthreads not even - there will be errors in the image! \n");
/* Pthread options */
pthread_attr_init(&attr);
pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
/* Create the render threads */
for (t=0; t<NUMTHREADS; t++) {
tinfo[t].thread_num = t;
if (pthread_create(&tinfo[t].thread_id, &attr, renderThread, &tinfo[t])) {
printf("Creation of thread %d failed. \n", t);
exit(-1);
}
}
if (pthread_attr_destroy(&attr)) {
printf("Error destroying thread attributes. \n");
}
/* Wait for render threads to finish */
for (t=0; t<NUMTHREADS; t++) {
if (pthread_join(tinfo[t].thread_id, NULL)) {
printf("Error waiting for thread. \n");
}
}
/* Save the image */
savebmp("out.bmp", img, myScene);
/* Free allocated memory */
if (img)
free(img);
if (myScene->lights)
free(myScene->lights);
if (myScene->triangles)
free(myScene->triangles);
if (myScene->spheres)
free(myScene->spheres);
if (myScene->materials)
free(myScene->materials);
/* Exit */
pthread_exit(NULL);
if (tinfo)
free(tinfo);
return 0;
}
void *renderThread(void *arg)
{
int x,y;
thread_info *tinfo = (thread_info *)arg;
/* Calculate which part to render based on thread id */
int limits[]={(tinfo->thread_num*sectionsize),(tinfo->thread_num*sectionsize)+sectionsize};
/* Autoexposure */
double exposure = AutoExposure(myScene);
double projectionDistance = 0.0f;
if ((myScene->persp.type == CONIC) && myScene->persp.FOV > 0.0f && myScene->persp.FOV < 189.0f) {
projectionDistance = 0.5f * myScene->width / tanf((double)(PIOVER180) * 0.5f * myScene->persp.FOV);
}
for (y = limits[0]; y < limits[1]; ++y) {
for (x = 0; x < myScene->width; ++x) {
colour output = {0.0f, 0.0f, 0.0f};
double fragmentx, fragmenty;
/* Antialiasing */
for (fragmentx = x; fragmentx < x + 1.0f; fragmentx += 0.5f) {
for (fragmenty = y; fragmenty < y + 1.0f; fragmenty += 0.5f) {
double sampleRatio=0.25f;
colour temp = {0.0f, 0.0f, 0.0f};
double totalWeight = 0.0f;
if (myScene->persp.type == ORTHOGONAL || projectionDistance == 0.0f) {
ray viewRay = { {fragmentx, fragmenty, -10000.0f},{ 0.0f, 0.0f, 1.0f}};
int i;
for (i = 0; i < myScene->complexity; ++i) {
colour result = raytrace(&viewRay, myScene);
totalWeight += 1.0f;
temp = colourAdd(&temp,&result);
}
temp = colourCoefMul((1.0f/totalWeight), &temp);
} else {
vector dir = {(fragmentx - 0.5f * myScene->width) / projectionDistance,
(fragmenty - 0.5f * myScene->height) / projectionDistance,
1.0f
};
double norm = vectorDot(&dir,&dir);
if (norm == 0.0f)
break;
dir = vectorScale(invsqrtf(norm),&dir);
vector start = {0.5f * myScene->width, 0.5f * myScene->height, 0.0f};
vector tmp = vectorScale(myScene->persp.clearPoint,&dir);
vector observed = vectorAdd(&start,&tmp);
int i;
for (i = 0; i < myScene->complexity; ++i) {
ray viewRay = { {start.x, start.y, start.z}, {dir.x, dir.y, dir.z} };
if (myScene->persp.dispersion != 0.0f) {
vector disturbance;
disturbance.x = (myScene->persp.dispersion / RAND_MAX) * (1.0f * rand());
disturbance.y = (myScene->persp.dispersion / RAND_MAX) * (1.0f * rand());
disturbance.z = 0.0f;
viewRay.start = vectorAdd(&viewRay.start, &disturbance);
viewRay.dir = vectorSub(&observed, &viewRay.start);
norm = vectorDot(&viewRay.dir,&viewRay.dir);
if (norm == 0.0f)
break;
viewRay.dir = vectorScale(invsqrtf(norm),&viewRay.dir);
}
colour result = raytrace(&viewRay, myScene);
totalWeight += 1.0f;
temp = colourAdd(&temp,&result);
}
temp = colourCoefMul((1.0f/totalWeight), &temp);
}
temp.blue = (1.0f - expf(temp.blue * exposure));
temp.red = (1.0f - expf(temp.red * exposure));
temp.green = (1.0f - expf(temp.green * exposure));
colour adjusted = colourCoefMul(sampleRatio, &temp);
output = colourAdd(&output, &adjusted);
}
}
/* gamma correction */
double invgamma = 0.45f; //Fixed value from sRGB standard
output.blue = powf(output.blue, invgamma);
output.red = powf(output.red, invgamma);
output.green = powf(output.green, invgamma);
img[(x+y*myScene->width)*3+2] = (unsigned char)min(output.red*255.0f, 255.0f);
img[(x+y*myScene->width)*3+1] = (unsigned char)min(output.green*255.0f, 255.0f);
img[(x+y*myScene->width)*3+0] = (unsigned char)min(output.blue*255.0f,255.0f);
}
}
pthread_exit((void *) arg);
}
colour raytrace(ray *viewRay, scene *myScene)
{
colour output = {0.0f, 0.0f, 0.0f};
double coef = 1.0f;
int level = 0;
do {
vector hitpoint,n;
int currentSphere = -1;
int currentTriangle = -1;
material currentMat;
double t = 20000.0f;
double temp;
vector n1;
unsigned int i;
for (i = 0; i < myScene->numSpheres; ++i) {
if (collideRaySphere(viewRay, &myScene->spheres[i], &t)) {
currentSphere = i;
}
}
for (i = 0; i < myScene->numTriangles; ++i) {
if (collideRayTriangle(viewRay, &myScene->triangles[i], &t, &n1)) {
currentTriangle = i;
currentSphere = -1;
}
}
if (currentSphere != -1) {
vector scaled = vectorScale(t, &viewRay->dir);
hitpoint = vectorAdd(&viewRay->start, &scaled);
n = vectorSub(&hitpoint, &myScene->spheres[currentSphere].pos);
temp = vectorDot(&n,&n);
if (temp == 0.0f) break;
temp = invsqrtf(temp);
n = vectorScale(temp, &n);
currentMat = myScene->materials[myScene->spheres[currentSphere].material];
} else if (currentTriangle != -1) {
vector scaled = vectorScale(t, &viewRay->dir);
hitpoint = vectorAdd(&viewRay->start, &scaled);
n=n1;
temp = vectorDot(&n,&n);
if (temp == 0.0f) break;
temp = invsqrtf(temp);
n = vectorScale(temp, &n);
currentMat = myScene->materials[myScene->triangles[currentTriangle].material];
} else {
/* No hit - add background */
colour test = {0.05,0.05,0.35};
colour tmp = colourCoefMul(coef, &test);
output = colourAdd(&output,&tmp);
break;
}
/* Bump mapping using Perlin noise */
if (currentMat.bump != 0.0) {
double noiseCoefx = noise(0.1 * hitpoint.x, 0.1 * hitpoint.y, 0.1 * hitpoint.z, myNoise);
double noiseCoefy = noise(0.1 * hitpoint.y, 0.1 * hitpoint.z, 0.1 * hitpoint.x, myNoise);
double noiseCoefz = noise(0.1 * hitpoint.z, 0.1 * hitpoint.x, 0.1 * hitpoint.y, myNoise);
n.x = (1.0f - currentMat.bump ) * n.x + currentMat.bump * noiseCoefx;
n.y = (1.0f - currentMat.bump ) * n.y + currentMat.bump * noiseCoefy;
n.z = (1.0f - currentMat.bump ) * n.z + currentMat.bump * noiseCoefz;
temp = vectorDot(&n, &n);
if (temp == 0.0f) {
break;
}
temp = invsqrtf(temp);
n = vectorScale(temp, &n);
}
bool inside;
if (vectorDot(&n,&viewRay->dir) > 0.0f) {
n = vectorScale(-1.0f,&n);
inside = TRUE;
} else {
inside = FALSE;
}
if (!inside) {
ray lightRay;
lightRay.start = hitpoint;
/* Find the value of the light at this point */
unsigned int j;
for (j = 0; j < myScene->numLights; ++j) {
light currentLight = myScene->lights[j];
lightRay.dir = vectorSub(¤tLight.pos,&hitpoint);
double lightprojection = vectorDot(&lightRay.dir,&n);
if ( lightprojection <= 0.0f ) continue;
double lightdist = vectorDot(&lightRay.dir,&lightRay.dir);
double temp = lightdist;
if ( temp == 0.0f ) continue;
temp = invsqrtf(temp);
lightRay.dir = vectorScale(temp,&lightRay.dir);
lightprojection = temp * lightprojection;
/* Calculate the shadows */
bool inshadow = FALSE;
double t = lightdist;
unsigned int k;
for (k = 0; k < myScene->numSpheres; ++k) {
if (collideRaySphere(&lightRay, &myScene->spheres[k], &t)) {
inshadow = TRUE;
break;
}
}
for (k = 0; k < myScene->numTriangles; ++k) {
if (collideRayTriangle(&lightRay, &myScene->triangles[k], &t, &n1)) {
inshadow = TRUE;
break;
}
}
if (!inshadow) {
/* Diffuse refraction using Lambert */
double lambert = vectorDot(&lightRay.dir, &n) * coef;
double noiseCoef = 0.0f;
int level = 0;
switch (currentMat.MatType) {
case TURBULENCE:
for (level = 1; level < 10; level++) {
noiseCoef += (1.0f / level )
* fabsf(noise(level * 0.05 * hitpoint.x,
level * 0.05 * hitpoint.y,
level * 0.05 * hitpoint.z,
myNoise));
}
colour diff1 = colourCoefMul(noiseCoef,¤tMat.diffuse);
colour diff2 = colourCoefMul((1.0f - noiseCoef), ¤tMat.mdiffuse);
colour temp1 = colourAdd(&diff1, &diff2);
output = colourAdd(&output, &temp1);
break;
case MARBLE:
for (level = 1; level < 10; level ++) {
noiseCoef += (1.0f / level)
* fabsf(noise(level * 0.05 * hitpoint.x,
level * 0.05 * hitpoint.y,
level * 0.05 * hitpoint.z,
myNoise));
}
noiseCoef = 0.5f * sinf( (hitpoint.x + hitpoint.y) * 0.05f + noiseCoef) + 0.5f;
colour diff3 = colourCoefMul(noiseCoef,¤tMat.diffuse);
colour diff4 = colourCoefMul((1.0f - noiseCoef), ¤tMat.mdiffuse);
colour tmp1 = colourAdd(&diff3, &diff4);
colour lamint = colourCoefMul(lambert, ¤tLight.intensity);
colour lamint_scaled = colourCoefMul(coef, &lamint);
colour temp2 = colourMul(&lamint_scaled, &tmp1);
output = colourAdd(&output, &temp2);
break;
/* Basically Gouraud */
default:
output.red += lambert * currentLight.intensity.red * currentMat.diffuse.red;
output.green += lambert * currentLight.intensity.green * currentMat.diffuse.green;
output.blue += lambert * currentLight.intensity.blue * currentMat.diffuse.blue;
break;
}
/* Blinn */
double viewprojection = vectorDot(&viewRay->dir, &n);
vector blinnDir = vectorSub(&lightRay.dir, &viewRay->dir);
double temp = vectorDot(&blinnDir, &blinnDir);
if (temp != 0.0f ) {
double blinn = invsqrtf(temp) * max(lightprojection - viewprojection , 0.0f);
blinn = coef * powf(blinn, currentMat.power);
colour tmp_1 = colourCoefMul(blinn, ¤tMat.specular);
colour tmp_2 = colourMul(&tmp_1, ¤tLight.intensity);
output = colourAdd(&output, &tmp_2);
}
}
}
/* Iterate over the reflection */
coef *= currentMat.reflection;
double reflect = 2.0f * vectorDot(&viewRay->dir, &n);
viewRay->start = hitpoint;
vector tmp = vectorScale(reflect, &n);
viewRay->dir = vectorSub(&viewRay->dir, &tmp);
}
level++;
/* Limit iteration depth to 10 */
} while ((coef > 0.0f) && (level < 10));
return output;
}
bool handleCollision(Contact *contact) {
Object *source = contact->object1;
Object *target = contact->object2;
float *normal = contact->normal;
float *contactpoint = contact->position;
float sourcevelocity[3], targetvelocity[3];
float sourcecontactpoint[3], targetcontactpoint[3];
vectorSub(sourcecontactpoint, contactpoint, source->position);
source->getVelocity(sourcevelocity, sourcecontactpoint);
if (target == NULL) {
vectorSet(targetcontactpoint, 0, 0, 0);
vectorSet(targetvelocity, 0, 0, 0);
} else {
vectorSub(targetcontactpoint, contactpoint, target->position);
target->getVelocity(targetvelocity, targetcontactpoint);
}
float deltavelocity[3];
vectorSub(deltavelocity, sourcevelocity, targetvelocity);
float dot = vectorDot(deltavelocity, normal);
// if (fabs(dot) < EPSILON) return false;
// if (dot > -1.0e-5 && dot < 1.0e-5) return false;
// if (dot >= 0) return false;
if (dot > -1.0e-5)
return false;
float invmass1;
invmass1 = source->invmass;
float invmass2;
if (target == NULL)
invmass2 = 0;
else
invmass2 = target->invmass;
float t1;
if (source->invmomentofinertia == 0) {
t1 = 0;
} else {
float v1[3];
vectorCross(v1, sourcecontactpoint, normal);
vectorScale(v1, source->invmomentofinertia);
float w1[3];
vectorCross(w1, v1, sourcecontactpoint);
t1 = vectorDot(normal, w1);
}
float t2;
if (target == NULL || target->invmomentofinertia == 0) {
t2 = 0;
} else {
float v1[3];
vectorCross(v1, targetcontactpoint, normal);
vectorScale(v1, target->invmomentofinertia);
float w1[3];
vectorCross(w1, v1, targetcontactpoint);
t2 = vectorDot(normal, w1);
}
float denominator = invmass1 + invmass2 + t1 + t2;
float e = 1.0 - COLLISIONFRICTION;
float impulsesize = (1 + e) * dot / denominator;
// printf("%f\n", impulsesize);
float impulse[3];
vectorScale(impulse, normal, impulsesize);
float friction[3];
vectorScale(friction, normal, vectorDot(deltavelocity, normal));
vectorAdd(friction, deltavelocity);
vectorNormalize(friction);
float frictionsize = 10 * KINETICFRICTION * dot / denominator;
float maxfrictionsize = 0.1 * vectorLength(deltavelocity);
if (frictionsize < -maxfrictionsize)
frictionsize = -maxfrictionsize;
vectorScale(friction, -frictionsize);
vectorAdd(impulse, friction);
if (target != NULL) {
target->addImpulse(impulse, targetcontactpoint);
target->calculateStateVariables();
}
float speed;
float speed2[3];
if (target != NULL && source != NULL) {
// float kvel[3];
// source->getVelocity(kvel);
float k = vectorLength(sourcevelocity) * 0.1;
// if (k > 1) k = 1;
speed = -impulsesize * target->invmass * k;
vectorScale(speed2, impulse, target->invmass * k);
/*float kvel[3];
source->getVelocity(kvel);
float k = 0;//vectorDot(speed2, kvel);
if (k < EPSILON) k = 0;
speed *= k;
vectorScale(speed2, k);
if (k > 0) */ target->hitForce(speed, speed2, source);
}
vectorScale(impulse, -1);
source->addImpulse(impulse, sourcecontactpoint);
source->calculateStateVariables();
// vectorScale(speed, source->invmass);
if (target != NULL && source != NULL) {
// float kvel[3];
// target->getVelocity(kvel);
float k = vectorLength(targetvelocity) * 0.1;
// if (k > 1) k = 1;
speed = -impulsesize * source->invmass * k;
vectorScale(speed2, impulse, source->invmass * k);
/*float kvel[3];
target->getVelocity(kvel);
float k = 0;//vectorDot(speed2, kvel);
if (k < EPSILON) k = 0;
speed *= k;
vectorScale(speed2, k);
if (k > 0) */ source->hitForce(speed, speed2, target);
}
return true;
}
bool checkSphereMeshCollision(float *sphereposition, float r, Mesh *mesh,
float *normal, float *contactpoint) {
float linenormal[3];
float pointnormal[3];
float maxdist = 0;
bool planecollision = false;
bool linecollision = false;
bool pointcollision = false;
int i, j;
for (i = 0; i < mesh->polygoncount; i++) {
class Polygon *polygon = &mesh->polygons[i];
float dist = distanceFromPlane(sphereposition, polygon->planenormal,
polygon->planedistance);
if (dist < r && dist > -r) {
bool directcollision = true;
for (j = 0; j < polygon->vertexcount; j++) {
float *p1 = polygon->vertices[j]->position;
float *p2 = polygon->vertices[(j + 1) % polygon->vertexcount]->position;
float *p3 = polygon->vertices[(j + 2) % polygon->vertexcount]->position;
float v1[3], v2[3];
vectorSub(v1, p2, p1);
// Collision for polygon surface
vectorSub(v2, p3, p2);
float t1[3];
vectorProject(t1, v2, v1);
float norm[3];
vectorSub(norm, v2, t1);
vectorNormalize(norm);
// Collision for polygon edges
float newpoint[3];
vectorSub(newpoint, sphereposition, p1);
float dist2 = vectorDot(newpoint, norm);
if (dist2 < 0) {
directcollision = false;
float projloc = vectorDot(newpoint, v1) / vectorDot(v1, v1);
if (projloc >= 0 && projloc <= 1) {
float proj[3];
vectorScale(proj, v1, projloc);
float projorth[3];
vectorSub(projorth, newpoint, proj);
float l2 = vectorDot(projorth, projorth);
if (l2 < r * r) {
vectorNormalize(linenormal, projorth);
if (dist < 0)
vectorScale(linenormal, -1);
linecollision = true;
}
}
}
// Collision for polygon vertices
float pointdiff[3];
vectorSub(pointdiff, sphereposition, p1);
float l3 = vectorDot(pointdiff, pointdiff);
if (l3 < r * r) {
vectorScale(pointnormal, pointdiff, 1.0 / sqrt(l3));
if (dist < 0)
vectorScale(pointnormal, -1);
pointcollision = true;
}
}
if (directcollision) {
if (dist > maxdist || !planecollision) {
vectorCopy(normal, polygon->planenormal);
maxdist = dist;
planecollision = true;
}
}
}
}
if (planecollision) {
vectorScale(contactpoint, normal, -r);
vectorAdd(contactpoint, sphereposition);
} else if (linecollision) {
vectorScale(contactpoint, linenormal, -r);
vectorAdd(contactpoint, sphereposition);
vectorCopy(normal, linenormal);
} else if (pointcollision) {
vectorScale(contactpoint, pointnormal, -r);
vectorAdd(contactpoint, sphereposition);
vectorCopy(normal, pointnormal);
} else {
return false;
}
return true;
}