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); }