struct BoundingBox newBoundingBox(void) { struct BoundingBox bb; bb.position = vectorZero(); bb.direction = vectorZero(); bb.direction.x = 1; bb.up = vectorZero(); bb.up.y = 1; bb.halfSize.x = 1; bb.halfSize.y = 1; bb.halfSize.z = 1; return bb; }
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 }
struct Vector vectorNormalize(struct Vector a) { struct Vector result; double length = vectorLength(a); if(length < EPSILON) return vectorZero(); result.x = a.x/length; result.y = a.y/length; result.z = a.z/length; return result; }
//tries to add a solution in the repository if it is non-dominated //param - the candidate solution to be inserted in the repository bool Repository::add(Solution &candidate){ bool isDominated=false; bool equal=false; int dom; bool enteredArchive=false; if(actualSize==0){ //if the repository is empty, insert the solution insert(candidate); enteredArchive=true; }else{ for(int s=0;s<repositorySize+1;s++){ if(controlSolutions[s]){//if this solution is valid if(!candidate.isEqual(getSolution(s))){ //if the solutions are not equal //verify the dominance relation between two vectors //return 1 if sol1 dominates sol2, -1 if sol2 dominates sol1, 0 if they do not dominate each other, 2 if they are equal if(!strcmp(archiverType, "pbi") || !strcmp(archiverType, "tch") || !strcmp(archiverType, "wcp") || !strcmp(archiverType, "wsum") || !strcmp(archiverType, "r-ideal"))//repositories based on decomposition, if decomposition do not check dominance dom=0; else dom=dominance(candidate.objectiveVector, getSolution(s).objectiveVector, objectiveNumber); if(dom == 1){//if the candidate dominates the solution in the repository exclude(s); }else{ if(dom == -1){//if a solution in the repository dominates the candidate isDominated=true; if(vectorZero(getSolution(s).objectiveVector, objectiveNumber)){ fprintf(stderr, "\nERROR! Trying to insert in the repository a solution whose objectives are all 0\n"); exit(1); } break; } } }else{ //if the solutions are equal, discard the candidate equal=true; break; } } } if(!isDominated && !equal){ //if the solution is non-dominated candidate.dominated=false; if(actualSize<repositorySize){//if the repository is not empty nor full insert(candidate);//insert the solution enteredArchive=true; }else{ //if the repository is full enteredArchive=archiver(candidate); } }else{ candidate.dominated=true; enteredArchive=false; } } return enteredArchive; }
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); }