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