void object::pointAt(float *vector)
{
float *tempA;
tempA = new float[3];
vectorSub(tempA, vector, mOrigin);
angleFromVector(mAngle, tempA);
buildTrans();
delete []tempA;
}
/* Returns Positive on separation - gives distance*/
double boundingBoxSeparationTest(struct Vector L, struct BoundingBox a, struct BoundingBox b)
{
double R = fabs(vectorDot(L, vectorSub(a.position, b.position)));
double R0 = a.halfSize.x*fabs(vectorDot(L, a.direction)) +
a.halfSize.y*fabs(vectorDot(L, a.up)) +
a.halfSize.z*fabs(vectorDot(L, vectorCross(a.up, a.direction)));
double R1 = b.halfSize.x*fabs(vectorDot(L, b.direction)) +
b.halfSize.y*fabs(vectorDot(L, b.up)) +
b.halfSize.z*fabs(vectorDot(L, vectorCross(b.up, b.direction)));
return R - (R0 + R1);
}
float MeshShape::calculateMomentOfInertia(float *rotationvector) {
if (vectorDot(rotationvector, rotationvector) < EPSILON)
return 0;
int i;
float j = 0;
for (i = 0; i < mesh->vertexcount; i++) {
float proj[3];
vectorProject(proj, mesh->vertices[i].position, rotationvector);
vectorSub(proj, mesh->vertices[i].position, proj);
// float r = vectorLength(proj);
float r2 = vectorDot(proj, proj);
j += r2;
}
return j / i;
}
bool scene::occluded(float *vectA, float *vectB)
{
bool result;
float *tempA;
float dist;
tempA = new float[3];
vectorSub(tempA, vectB, vectA);
dist = vectorMod(tempA);
vectorNorm(tempA, tempA);
if (beam(vectA, tempA, dist) != -1)
result = true;
else
result = false;
delete []tempA;
return result;
}
void main() {
int i;
int start;
double c1[3] = {22.5, 12.8, 80.2};
double c2[3] = {81.0, 5.0, 56.1};
Vector *a = vectorNew(c1, 3);
Vector *b = vectorNew(c2, 3);
start = clock();
for (i=0; i<100000; i++) {
vectorAdd(a, b);
vectorSub(a, b);
vectorCompare(a, b);
angle(a, b);
vectorLength(a);
vectorLength(b);
dotProduct(a, b);
crossProduct(a, b);
}
printf("%f ", (float)(clock()-start)/CLOCKS_PER_SEC);
getchar();
}
void boatPersonFree(boat b) /*Joga as pessoas de volta no oceano*/
{
int i, errorCode;
listLink aux = b->extra->personList;
double angDif = 2 * PI / b->extra->peopleHeld;
for (i = 0; i < b->extra->peopleHeld; i++) {
if (aux == NULL)
genError
("Falha de sincronia entre personList e peopleHeld! Contate um programador.");
aux->person->pos =
vectorSum(b->pos,
vectorPolarToCartesian(b->radius +
aux->person->radius,
i * angDif));
aux->person->vel = vectorAngleSet(aux->person->vel, angDif);
if ((errorCode = objectTableAddObject(aux->person))) {
if (errorCode == ERROR_OBJECT_LIMIT_EXCEEDED)
genWarning
("Nao foi possivel fazer a pessoa cair do bote!\n");
if (errorCode == ERROR_OBJECT_IS_COLLIDING) {
do {
aux->person->pos =
vectorSum(aux->person->pos,
vectorMulDouble(vectorSub
(aux->person->pos,
b->pos), 1.1));
/*Se ele tentar colocar a pessoa num lugar ja ocupado, ele joga ela pra mais longe ate estar num lugar disponivel*/
} while (objectTableAddObject(aux->person) ==
ERROR_OBJECT_IS_COLLIDING);
}
}
aux = aux->next;
}
for (aux = b->extra->personList; aux != NULL;
aux = b->extra->personList) {
b->extra->personList = b->extra->personList->next;
free(aux);
b->extra->peopleHeld--;
}
}
void testVectors(){
vector<double> test1(10,8.0);
vector<double> abc(10,7.);
vector<double> res(10);
cout<<"vecadd test1+abc "<<endl;
vectorAdd(&test1,&abc,&res);
vectorPrint(&res);
cout<<"vecsub res-abc "<<endl;
vectorSub(&res,&abc,&res);
vectorPrint(&res);
cout<<"vecscalar 3 "<<endl;
vectorScalar(&res,3.);
vectorPrint(&res);
cout<<"vecscalar 3 "<<endl;
vectorScalar(&res,3.,&test1);
vectorPrint(&test1);
cout<<"vecvec test1 abc "<<endl;
cout<<vectorVector(&test1,&abc)<<endl;
vector<map< int,double> > testmat(2);//=new vector<map< int,double> >(n);
for (int i=0;i<2;++i){
testmat[i];//=new map< int,double> ();
}
testmat[0][0]= 1;
testmat[0][1]= 1;
testmat[1][0]= 1;
//testmat[1][1]= 1;
vector<double> vec(2,8.0);
vector<double> ret(2);
matrixVector(&testmat,&vec,&ret);
vectorPrint(&ret);
}
/* Check if a ray collides with a sphere */
bool collideRaySphere(ray *r, sphere *s, double *t)
{
/* Ray/Sphere intersection
*
* collideRaySpere(ray,sphere)
* Vector distance = ray.start - sphere.position
* B = distance dot ray.direction
* C = distance dot distance - sphere.radius^2
* Discriminant = B^2 - C
* Discriminant < 0 --> no collision
* Otherwise select closest solution
*/
vector dist = vectorSub(&r->start, &s->pos);
double B = vectorDot(&dist, &r->dir);
double D = B * B - vectorDot(&dist, &dist) + s->size * s->size;
/* Discriminant less than 0 ? */
if (D < 0.0f)
return FALSE;
double t0 = -B - sqrtf(D);
double t1 = -B + sqrtf(D);
bool retvalue = FALSE;
if ((t0 > 0.1f) && (t0 < *t)) {
*t = t0;
retvalue = TRUE;
}
if ((t1 > 0.1f) && (t1 < *t)) {
*t = t1;
retvalue = TRUE;
}
return retvalue;
}
/*
* computeMaxError :
* Find the maximum squared distance of digitized points
* to fitted curve.
*/
double computeMaxError(QPolygonF points,int first,int last,QPointF *curve,double *u,int *splitPoint)
{
int i;
double maxDist; /* Maximum error */
double dist; /* Current error */
QPointF P; /* Point on curve */
FitVector v; /* Vector from point to curve */
*splitPoint = (last - first + 1)/2;
maxDist = 0.0;
for (i = first + 1; i < last; i++) {
P = bezierII(3, curve, u[i-first]);
v = vectorSub(P, points[i]);
dist = v.length();
if (dist >= maxDist) {
maxDist = dist;
*splitPoint = i;
}
}
return (maxDist);
}
void boatCollide(boat b, object o, double timediff)
{
double objectSide; /*Variavel que guarda o tamanho de um dos lados do outro objeto, caso seja o Asimov ou um coral */
switch (o->type) {
case TYPE_BOAT:
if (vectorLength(o->extra->prevVel) != 0) {
b->vel = o->extra->prevVel;
o->extra->prevVel = vectorCreate(0, 0);
} else {
b->extra->prevVel = b->vel;
b->vel = o->vel;
}
b->pos =
vectorSum(vectorLengthSet
(vectorSub(b->pos, o->pos),
b->radius + o->radius + 1), o->pos);
break;
case TYPE_CORAL:
objectSide = o->radius * SQRT_2 / 2;
/*Note que a colisao so existe caso bata num dos lados do coral, nao se simplesmente estiver no circulo de colisao*/
if (((abs(b->pos.x - o->pos.x) <= objectSide
&& abs(b->pos.y - o->pos.y) <= (objectSide + b->radius))
|| (abs(b->pos.y - o->pos.y) <= objectSide
&& abs(b->pos.x - o->pos.x) <= (objectSide + b->radius))
||
((abs(b->pos.x - o->pos.x) >= objectSide
&& abs(b->pos.y - o->pos.y) >= objectSide)))
&& b->extra->timeStuckLeft == 0)
boatCrash(b);
break;
case TYPE_SHIP:
objectSide = o->radius / SQRT_5; /*Note que o retangulo e um retangulo
inscrito ao circulo de colisao */
/*Vide comentarios para colisao com coral */
if (abs(b->pos.x - o->pos.x) <= 2 * objectSide
&& abs(b->pos.y - o->pos.y) <= (objectSide + b->radius)) {
b->vel.y *= -1;
(b->pos.y >= o->pos.y) ? (b->pos.y =
o->pos.y + objectSide + b->radius +
1) : (b->pos.y =
o->pos.y - objectSide -
b->radius - 1);
} else if (abs(b->pos.y - o->pos.y) <= objectSide
&& abs(b->pos.x - o->pos.x) <=
(2 * objectSide + b->radius)) {
b->vel.x *= -1;
(b->pos.x >= o->pos.x) ? (b->pos.x =
o->pos.x + objectSide * 2 +
b->radius + 1) : (b->pos.x =
o->pos.x -
objectSide * 2 -
b->radius - 1);
} else if (abs(b->pos.x - o->pos.x) >= 2 * objectSide
&& abs(b->pos.y - o->pos.y) >= objectSide) {
b->vel.x *= -1;
b->vel.y *= -1;
b->pos =
vectorSum(vectorLengthSet
(vectorSub(b->pos, o->pos),
b->radius + o->radius + 1), o->pos);
}
break;
case TYPE_PERSON:
if (!boatFullOrCrashed(b))
personBoatCollision(b, o);
break;
default:
genWarning("Colisao de barco com tipo desconhecido!\n");
}
}
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 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;
}
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;
}
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;
}
void boundingBoxCollisionPoint(struct Vector* normal, struct Vector* point, struct BoundingBox ai, struct BoundingBox bi, struct BoundingBox af, struct BoundingBox bf)
{
int i;
int separartingAxesNum = 0;
int lastSeparartingAxis = 0;
struct BoundingBox a;
struct BoundingBox b;
double distance, minDistance;
i=1000;
while(separartingAxesNum != 1 && i-->0)
{
separartingAxesNum = 0;
a = boundingBoxInterpolate(&ai, &af);
b = boundingBoxInterpolate(&bi, &bf);
struct Vector a1 = a.direction = vectorNormalize(a.direction);
struct Vector a2 = a.up = vectorNormalize(a.up);
struct Vector a3 = vectorCross(a1, a2);
struct Vector b1 = b.direction = vectorNormalize(b.direction);
struct Vector b2 = b.up = vectorNormalize(b.up);
struct Vector b3 = vectorCross(b1, b2);
if(boundingBoxSeparationTest(a1, a, b)>0) {separartingAxesNum++; lastSeparartingAxis=0;}
if(boundingBoxSeparationTest(a2, a, b)>0) {separartingAxesNum++; lastSeparartingAxis=1;}
if(boundingBoxSeparationTest(a3, a, b)>0) {separartingAxesNum++; lastSeparartingAxis=2;}
if(boundingBoxSeparationTest(b1, a, b)>0) {separartingAxesNum++; lastSeparartingAxis=3;}
if(boundingBoxSeparationTest(b2, a, b)>0) {separartingAxesNum++; lastSeparartingAxis=4;}
if(boundingBoxSeparationTest(b3, a, b)>0) {separartingAxesNum++; lastSeparartingAxis=5;}
if(boundingBoxSeparationTest(vectorCross(a1, b1), a, b)>0) {separartingAxesNum++; lastSeparartingAxis=6;}
if(boundingBoxSeparationTest(vectorCross(a1, b2), a, b)>0) {separartingAxesNum++; lastSeparartingAxis=7;}
if(boundingBoxSeparationTest(vectorCross(a1, b3), a, b)>0) {separartingAxesNum++; lastSeparartingAxis=8;}
if(boundingBoxSeparationTest(vectorCross(a2, b1), a, b)>0) {separartingAxesNum++; lastSeparartingAxis=9;}
if(boundingBoxSeparationTest(vectorCross(a2, b2), a, b)>0) {separartingAxesNum++; lastSeparartingAxis=10;}
if(boundingBoxSeparationTest(vectorCross(a2, b3), a, b)>0) {separartingAxesNum++; lastSeparartingAxis=11;}
if(boundingBoxSeparationTest(vectorCross(a3, b1), a, b)>0) {separartingAxesNum++; lastSeparartingAxis=12;}
if(boundingBoxSeparationTest(vectorCross(a3, b2), a, b)>0) {separartingAxesNum++; lastSeparartingAxis=13;}
if(boundingBoxSeparationTest(vectorCross(a3, b3), a, b)>0) {separartingAxesNum++; lastSeparartingAxis=14;}
if(separartingAxesNum>1) // is not colliding yet
{
ai = a;
bi = b;
}
if(separartingAxesNum<1) // is already colliding
{
af = a;
bf = b;
}
}
if(lastSeparartingAxis<6) // face-vertex
{
struct Vector center;
int pointId = 0;
struct Vector verticles[6];
if(lastSeparartingAxis<3) // b-vertex collide with a-face
{
boundingBoxGetVerticles(verticles, &b);
center = a.position;
}
else // a-vertex collide with b-face
{
boundingBoxGetVerticles(verticles, &a);
center = b.position;
}
switch(lastSeparartingAxis)
{
case 0:
*normal = a.direction;
distance = a.halfSize.x;
break;
case 1:
*normal = a.up;
distance = a.halfSize.y;
break;
case 2:
*normal = vectorCross(a.direction, a.up);
distance = a.halfSize.z;
break;
case 3:
*normal = b.direction;
distance = b.halfSize.x;
break;
case 4:
*normal = b.up;
distance = b.halfSize.y;
break;
case 5:
*normal = vectorCross(b.direction, b.up);;
distance = b.halfSize.z;
break;
default:
*normal = vectorZero();
distance = 0;
}
minDistance = -1;
for(i=0;i<6;i++)
{
distance = vectorPointPlaneDistance(verticles[i], center, *normal);
if(minDistance < 0 || distance < minDistance)
{
minDistance = distance;
pointId = i;
}
}
*point = verticles[pointId];
}
else // edge-edge
{
struct Vector a1 = a.direction = vectorNormalize(a.direction);
struct Vector a2 = a.up = vectorNormalize(a.up);
struct Vector a3 = vectorNormalize( vectorCross(a1, a2) );
struct Vector b1 = b.direction = vectorNormalize(b.direction);
struct Vector b2 = b.up = vectorNormalize(b.up);
struct Vector b3 = vectorNormalize(vectorCross(b1, b2));
struct Vector halfEdgeA, halfEdgeB;
struct Vector halfOffsetA[4];
struct Vector halfOffsetB[4];
for(i=0;i<4;i++)
{switch(lastSeparartingAxis)
{
case 6: case 7: case 8:
halfEdgeA = vectorTimes(a1, a.halfSize.x);
halfOffsetA[i] = vectorAdd( vectorTimes(a1, 0),
vectorAdd( vectorTimes(a2, a.halfSize.y * (1-2*(i%2)) ),
vectorTimes(a3, a.halfSize.z * (i>=2 ? -1 : 1) )));
break;
case 9: case 10: case 11:
halfEdgeA = vectorTimes(a2, a.halfSize.y);
halfOffsetA[i] = vectorAdd( vectorTimes(a1, a.halfSize.x * (1-2*(i%2))),
vectorAdd( vectorTimes(a2, 0),
vectorTimes(a3, a.halfSize.z * (i>=2 ? -1 : 1))));
break;
case 12: case 13: case 14:
halfEdgeA = vectorTimes(a3, a.halfSize.z);
halfOffsetA[i] = vectorAdd( vectorTimes(a1, a.halfSize.x * (1-2*(i%2))),
vectorAdd( vectorTimes(a2, a.halfSize.y * (i>=2 ? -1 : 1)),
vectorTimes(a3, 0)));
break;
}
switch(lastSeparartingAxis)
{
case 6: case 9: case 12:
halfEdgeB = vectorTimes(b1, b.halfSize.x);
halfOffsetB[i] = vectorAdd( vectorTimes(b1, 0),
vectorAdd( vectorTimes(b2, b.halfSize.y * (1-2*(i%2))),
vectorTimes(b3, b.halfSize.z * (i>=2 ? -1 : 1))));
break;
case 7: case 10: case 13:
halfEdgeB = vectorTimes(b2, b.halfSize.y);
halfOffsetB[i] = vectorAdd( vectorTimes(b1, b.halfSize.x * (1-2*(i%2))),
vectorAdd( vectorTimes(b2, 0),
vectorTimes(b3, b.halfSize.z * (i>=2 ? -1 : 1))));
break;
case 8: case 11: case 14:
halfEdgeB = vectorTimes(b3, b.halfSize.z);
halfOffsetB[i] = vectorAdd( vectorTimes(b1, b.halfSize.x * (1-2*(i%2))),
vectorAdd( vectorTimes(b2, b.halfSize.y * (i>=2 ? -1 : 1)),
vectorTimes(b3, 0)));
break;
}
}
*normal = vectorNormalize( vectorCross(halfEdgeA, halfEdgeB) );
int edgeAi, edgeBi;
int edgeA, edgeB;
minDistance = -1;
for(edgeAi = 0; edgeAi<4; edgeAi++)
for(edgeBi = 0; edgeBi<4; edgeBi++)
{
distance = vectorDot(*normal, vectorAdd(a.position, halfOffsetA[edgeAi])) - vectorDot(*normal, vectorAdd(b.position, halfOffsetB[edgeBi]));
distance = fabs(distance);
if(minDistance == -1 || distance < minDistance)
{
minDistance = distance;
edgeA = edgeAi;
edgeB = edgeBi;
}
}
struct Vector pointEdgeA = vectorAdd(a.position, halfOffsetA[edgeA]);
struct Vector pointEdgeB = vectorAdd(b.position, halfOffsetB[edgeB]);
struct Vector rayVector = vectorNormalize(halfEdgeA);
struct Vector rayPoint = pointEdgeA;
struct Vector planePoint = pointEdgeB;
struct Vector planeNormal = vectorNormalize(vectorCross(vectorNormalize(halfEdgeB), *normal));
// ray - plane intersection
double cosAlpha = vectorDot(rayVector, planeNormal);
double distance = vectorPointPlaneDistance(rayPoint, planePoint, planeNormal);
struct Vector pointA = vectorAdd(rayPoint, vectorTimes(rayVector, cosAlpha*distance)) ;
rayVector = vectorNormalize(halfEdgeB);
rayPoint = pointEdgeB;
planePoint = pointEdgeA;
planeNormal = vectorNormalize(vectorCross(vectorNormalize(halfEdgeA), *normal));
// ray - plane intersection
cosAlpha = vectorDot(rayVector, planeNormal);
distance = vectorPointPlaneDistance(rayPoint, planePoint, planeNormal);
struct Vector pointB = vectorAdd(rayPoint, vectorTimes(rayVector, cosAlpha*distance)) ;
*point = vectorTimes(vectorAdd(pointA, pointB),0.5);
}
/* normal should point in direction from A to B*/
double smallValue = 0.001;
if(vectorLength(vectorSub(vectorAdd(*point,vectorTimes(*normal, smallValue)), a.position))
<
vectorLength(vectorSub(vectorAdd(*point,vectorZero()), a.position))) /* |point + normal - A| < |point-A| */
*normal = vectorTimes(*normal, -1.0);
*normal = vectorNormalize(*normal);
}
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;
}
/**
* 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);
}
