int main(){
double *A, *Q, *R, *tmpQ, *B;
int i, j, k, N;
printf("\n Enter N = ");
scanf("%d",&N);
A = (double *)malloc(N*N*sizeof(double));
Q = (double *)malloc(N*N*sizeof(double));
R = (double *)malloc(N*N*sizeof(double));
tmpQ = (double *)malloc(N*N*sizeof(double));
B = (double *)malloc(N*N*sizeof(double));
for(i=0;i<N*N;i++) // input A
A[i] = i;
// QR-dcomposition
for(i=0;i<N*N;i++)
tmpQ[i] = A[i];
for(i=0;i<N;i++){
R[i*N+i] = sqrt(dot(tmpQ, tmpQ, N, i, i));
for(k=0;k<N;k++)
Q[i+N*k] = tmpQ[i+N*k]/R[i*N+i];
for(j=i+1;j<N;j++){
R[N*i+j] = dot(Q, tmpQ, N, i, j);
for(k=0;k<N;k++)
tmpQ[j+N*k] = tmpQ[j+N*k]-R[N*i+j]*Q[i+N*k];
}
}
MxM(Q,R,B,N); // Q*R=B
output(A, Q, R, B, N);
return 0;
}
int main(int argc, char** argv)
{
if (argc < 3) {
printf("need two parameters, the matrix size and the number of vectors\n");
return 1;
}
int N=atoi(argv[1]);
int K=atoi(argv[2]);
Matrix A = createMatrix(N,N);
// identity matrix
for (int i=0;i<N;++i)
A->data[i][i] = 1.0;
Matrix v = createMatrix(N,K);
// fill with column number
for (int i=0;i<K;++i)
for (int j=0;j<N;++j)
v->data[i][j] = i;
Matrix v2 = createMatrix(N,K);
double time = WallTime();
MxM(A, v, v2, 1.0, 0.0);
double sum = innerproduct(v->as_vec, v2->as_vec);
printf("sum: %f\n", sum);
printf("elapsed: %f\n", WallTime()-time);
freeMatrix(v2);
freeMatrix(v);
freeMatrix(A);
return 0;
}
int
PQP_Collide(PQP_CollideResult *res,
PQP_REAL R1[3][3], PQP_REAL T1[3], PQP_Model *o1,
PQP_REAL R2[3][3], PQP_REAL T2[3], PQP_Model *o2,
int flag)
{
double t1 = GetTime();
// make sure that the models are built
if (o1->build_state != PQP_BUILD_STATE_PROCESSED)
return PQP_ERR_UNPROCESSED_MODEL;
if (o2->build_state != PQP_BUILD_STATE_PROCESSED)
return PQP_ERR_UNPROCESSED_MODEL;
// clear the stats
res->num_bv_tests = 0;
res->num_tri_tests = 0;
// don't release the memory, but reset the num_pairs counter
res->num_pairs = 0;
// Okay, compute what transform [R,T] that takes us from cs1 to cs2.
// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
// First compute the rotation part, then translation part
MTxM(res->R,R1,R2);
PQP_REAL Ttemp[3];
VmV(Ttemp, T2, T1);
MTxV(res->T, R1, Ttemp);
// compute the transform from o1->child(0) to o2->child(0)
PQP_REAL Rtemp[3][3], R[3][3], T[3];
MxM(Rtemp,res->R,o2->child(0)->R);
MTxM(R,o1->child(0)->R,Rtemp);
#if PQP_BV_TYPE & OBB_TYPE
MxVpV(Ttemp,res->R,o2->child(0)->To,res->T);
VmV(Ttemp,Ttemp,o1->child(0)->To);
#else
MxVpV(Ttemp,res->R,o2->child(0)->Tr,res->T);
VmV(Ttemp,Ttemp,o1->child(0)->Tr);
#endif
MTxV(T,o1->child(0)->R,Ttemp);
// now start with both top level BVs
CollideRecurse(res,R,T,o1,0,o2,0,flag);
double t2 = GetTime();
res->query_time_secs = t2 - t1;
return PQP_OK;
}
int main(){
double *A, *B, *C, *D;
int i, N;
double t1, t2, t3, t4;
printf("\n Enter N = ");
scanf("%d",&N);
A = (double *)malloc(N*N*sizeof(double));
B = (double *)malloc(N*N*sizeof(double));
C = (double *)malloc(N*N*sizeof(double));
D = (double *)malloc(N*N*sizeof(double));
for(i=0;i<N*N;i++){ // input A B
A[i] = i+1;
B[i] = N*N-i;
}
t1 = clock();
MxM(A, B, C, N);
t2 = clock();
t3 = clock();
dgemm_row(A, B, D, N);
t4 = clock();
printf("\n Time = %f s\n",(t2-t1)/CLOCKS_PER_SEC);
printf(" Time_blas = %f s\n",(t4-t3)/CLOCKS_PER_SEC);
printf(" error = %e\n\n",err(C, D, N));
/*
printf(" C =\n");
print_matrix(C, N);
printf(" D =\n");
print_matrix(D, N);
*/
return 0;
}
int
PQP_Distance(PQP_DistanceResult *res,
PQP_REAL R1[3][3], PQP_REAL T1[3], PQP_Model *o1,
PQP_REAL R2[3][3], PQP_REAL T2[3], PQP_Model *o2,
PQP_REAL rel_err, PQP_REAL abs_err,
int qsize)
{
double time1 = GetTime();
// make sure that the models are built
if (o1->build_state != PQP_BUILD_STATE_PROCESSED)
return PQP_ERR_UNPROCESSED_MODEL;
if (o2->build_state != PQP_BUILD_STATE_PROCESSED)
return PQP_ERR_UNPROCESSED_MODEL;
// Okay, compute what transform [R,T] that takes us from cs2 to cs1.
// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
// First compute the rotation part, then translation part
MTxM(res->R,R1,R2);
PQP_REAL Ttemp[3];
VmV(Ttemp, T2, T1);
MTxV(res->T, R1, Ttemp);
// establish initial upper bound using last triangles which
// provided the minimum distance
PQP_REAL p[3],q[3];
res->distance = TriDistance(res->R,res->T,o1->last_tri,o2->last_tri,p,q);
VcV(res->p1,p);
VcV(res->p2,q);
// initialize error bounds
res->abs_err = abs_err;
res->rel_err = rel_err;
// clear the stats
res->num_bv_tests = 0;
res->num_tri_tests = 0;
// compute the transform from o1->child(0) to o2->child(0)
PQP_REAL Rtemp[3][3], R[3][3], T[3];
MxM(Rtemp,res->R,o2->child(0)->R);
MTxM(R,o1->child(0)->R,Rtemp);
#if PQP_BV_TYPE & RSS_TYPE
MxVpV(Ttemp,res->R,o2->child(0)->Tr,res->T);
VmV(Ttemp,Ttemp,o1->child(0)->Tr);
#else
MxVpV(Ttemp,res->R,o2->child(0)->To,res->T);
VmV(Ttemp,Ttemp,o1->child(0)->To);
#endif
MTxV(T,o1->child(0)->R,Ttemp);
// choose routine according to queue size
if (qsize <= 2)
{
DistanceRecurse(res,R,T,o1,0,o2,0);
}
else
{
res->qsize = qsize;
DistanceQueueRecurse(res,R,T,o1,0,o2,0);
}
// res->p2 is in cs 1 ; transform it to cs 2
PQP_REAL u[3];
VmV(u, res->p2, res->T);
MTxV(res->p2, res->R, u);
double time2 = GetTime();
res->query_time_secs = time2 - time1;
return PQP_OK;
}
void
DistanceQueueRecurse(PQP_DistanceResult *res,
PQP_REAL R[3][3], PQP_REAL T[3],
PQP_Model *o1, int b1,
PQP_Model *o2, int b2)
{
BVTQ bvtq(res->qsize);
BVT min_test;
min_test.b1 = b1;
min_test.b2 = b2;
McM(min_test.R,R);
VcV(min_test.T,T);
while(1)
{
int l1 = o1->child(min_test.b1)->Leaf();
int l2 = o2->child(min_test.b2)->Leaf();
if (l1 && l2)
{
// both leaves. Test the triangles beneath them.
res->num_tri_tests++;
PQP_REAL p[3], q[3];
Tri *t1 = &o1->tris[-o1->child(min_test.b1)->first_child - 1];
Tri *t2 = &o2->tris[-o2->child(min_test.b2)->first_child - 1];
PQP_REAL d = TriDistance(res->R,res->T,t1,t2,p,q);
if (d < res->distance)
{
res->distance = d;
VcV(res->p1, p); // p already in c.s. 1
VcV(res->p2, q); // q must be transformed
// into c.s. 2 later
o1->last_tri = t1;
o2->last_tri = t2;
}
}
else if (bvtq.GetNumTests() == bvtq.GetSize() - 1)
{
// queue can't get two more tests, recur
DistanceQueueRecurse(res,min_test.R,min_test.T,
o1,min_test.b1,o2,min_test.b2);
}
else
{
// decide how to descend to children
PQP_REAL sz1 = o1->child(min_test.b1)->GetSize();
PQP_REAL sz2 = o2->child(min_test.b2)->GetSize();
res->num_bv_tests += 2;
BVT bvt1,bvt2;
PQP_REAL Ttemp[3];
if (l2 || (!l1 && (sz1 > sz2)))
{
// put new tests on queue consisting of min_test.b2
// with children of min_test.b1
int c1 = o1->child(min_test.b1)->first_child;
int c2 = c1 + 1;
// init bv test 1
bvt1.b1 = c1;
bvt1.b2 = min_test.b2;
MTxM(bvt1.R,o1->child(c1)->R,min_test.R);
#if PQP_BV_TYPE & RSS_TYPE
VmV(Ttemp,min_test.T,o1->child(c1)->Tr);
#else
VmV(Ttemp,min_test.T,o1->child(c1)->To);
#endif
MTxV(bvt1.T,o1->child(c1)->R,Ttemp);
bvt1.d = BV_Distance(bvt1.R,bvt1.T,
o1->child(bvt1.b1),o2->child(bvt1.b2));
// init bv test 2
bvt2.b1 = c2;
bvt2.b2 = min_test.b2;
MTxM(bvt2.R,o1->child(c2)->R,min_test.R);
#if PQP_BV_TYPE & RSS_TYPE
VmV(Ttemp,min_test.T,o1->child(c2)->Tr);
#else
VmV(Ttemp,min_test.T,o1->child(c2)->To);
#endif
MTxV(bvt2.T,o1->child(c2)->R,Ttemp);
bvt2.d = BV_Distance(bvt2.R,bvt2.T,
o1->child(bvt2.b1),o2->child(bvt2.b2));
}
else
{
// put new tests on queue consisting of min_test.b1
// with children of min_test.b2
int c1 = o2->child(min_test.b2)->first_child;
int c2 = c1 + 1;
// init bv test 1
bvt1.b1 = min_test.b1;
bvt1.b2 = c1;
MxM(bvt1.R,min_test.R,o2->child(c1)->R);
#if PQP_BV_TYPE & RSS_TYPE
MxVpV(bvt1.T,min_test.R,o2->child(c1)->Tr,min_test.T);
#else
MxVpV(bvt1.T,min_test.R,o2->child(c1)->To,min_test.T);
#endif
bvt1.d = BV_Distance(bvt1.R,bvt1.T,
o1->child(bvt1.b1),o2->child(bvt1.b2));
// init bv test 2
bvt2.b1 = min_test.b1;
bvt2.b2 = c2;
MxM(bvt2.R,min_test.R,o2->child(c2)->R);
#if PQP_BV_TYPE & RSS_TYPE
MxVpV(bvt2.T,min_test.R,o2->child(c2)->Tr,min_test.T);
#else
MxVpV(bvt2.T,min_test.R,o2->child(c2)->To,min_test.T);
#endif
bvt2.d = BV_Distance(bvt2.R,bvt2.T,
o1->child(bvt2.b1),o2->child(bvt2.b2));
}
bvtq.AddTest(bvt1);
bvtq.AddTest(bvt2);
}
if (bvtq.Empty())
{
break;
}
else
{
min_test = bvtq.ExtractMinTest();
if ((min_test.d + res->abs_err >= res->distance) &&
((min_test.d * (1 + res->rel_err)) >= res->distance))
{
break;
}
}
}
}
void
DistanceRecurse(PQP_DistanceResult *res,
PQP_REAL R[3][3], PQP_REAL T[3], // b2 relative to b1
PQP_Model *o1, int b1,
PQP_Model *o2, int b2)
{
PQP_REAL sz1 = o1->child(b1)->GetSize();
PQP_REAL sz2 = o2->child(b2)->GetSize();
int l1 = o1->child(b1)->Leaf();
int l2 = o2->child(b2)->Leaf();
if (l1 && l2)
{
// both leaves. Test the triangles beneath them.
res->num_tri_tests++;
PQP_REAL p[3], q[3];
Tri *t1 = &o1->tris[-o1->child(b1)->first_child - 1];
Tri *t2 = &o2->tris[-o2->child(b2)->first_child - 1];
PQP_REAL d = TriDistance(res->R,res->T,t1,t2,p,q);
if (d < res->distance)
{
res->distance = d;
VcV(res->p1, p); // p already in c.s. 1
VcV(res->p2, q); // q must be transformed
// into c.s. 2 later
o1->last_tri = t1;
o2->last_tri = t2;
}
return;
}
// First, perform distance tests on the children. Then traverse
// them recursively, but test the closer pair first, the further
// pair second.
int a1,a2,c1,c2; // new bv tests 'a' and 'c'
PQP_REAL R1[3][3], T1[3], R2[3][3], T2[3], Ttemp[3];
if (l2 || (!l1 && (sz1 > sz2)))
{
// visit the children of b1
a1 = o1->child(b1)->first_child;
a2 = b2;
c1 = o1->child(b1)->first_child+1;
c2 = b2;
MTxM(R1,o1->child(a1)->R,R);
#if PQP_BV_TYPE & RSS_TYPE
VmV(Ttemp,T,o1->child(a1)->Tr);
#else
VmV(Ttemp,T,o1->child(a1)->To);
#endif
MTxV(T1,o1->child(a1)->R,Ttemp);
MTxM(R2,o1->child(c1)->R,R);
#if PQP_BV_TYPE & RSS_TYPE
VmV(Ttemp,T,o1->child(c1)->Tr);
#else
VmV(Ttemp,T,o1->child(c1)->To);
#endif
MTxV(T2,o1->child(c1)->R,Ttemp);
}
else
{
// visit the children of b2
a1 = b1;
a2 = o2->child(b2)->first_child;
c1 = b1;
c2 = o2->child(b2)->first_child+1;
MxM(R1,R,o2->child(a2)->R);
#if PQP_BV_TYPE & RSS_TYPE
MxVpV(T1,R,o2->child(a2)->Tr,T);
#else
MxVpV(T1,R,o2->child(a2)->To,T);
#endif
MxM(R2,R,o2->child(c2)->R);
#if PQP_BV_TYPE & RSS_TYPE
MxVpV(T2,R,o2->child(c2)->Tr,T);
#else
MxVpV(T2,R,o2->child(c2)->To,T);
#endif
}
res->num_bv_tests += 2;
PQP_REAL d1 = BV_Distance(R1, T1, o1->child(a1), o2->child(a2));
PQP_REAL d2 = BV_Distance(R2, T2, o1->child(c1), o2->child(c2));
if (d2 < d1)
{
if ((d2 < (res->distance - res->abs_err)) ||
(d2*(1 + res->rel_err) < res->distance))
{
DistanceRecurse(res, R2, T2, o1, c1, o2, c2);
}
if ((d1 < (res->distance - res->abs_err)) ||
(d1*(1 + res->rel_err) < res->distance))
{
DistanceRecurse(res, R1, T1, o1, a1, o2, a2);
}
}
else
{
if ((d1 < (res->distance - res->abs_err)) ||
(d1*(1 + res->rel_err) < res->distance))
{
DistanceRecurse(res, R1, T1, o1, a1, o2, a2);
}
if ((d2 < (res->distance - res->abs_err)) ||
(d2*(1 + res->rel_err) < res->distance))
{
DistanceRecurse(res, R2, T2, o1, c1, o2, c2);
}
}
}
void
CollideRecurse(PQP_CollideResult *res,
PQP_REAL R[3][3], PQP_REAL T[3], // b2 relative to b1
PQP_Model *o1, int b1,
PQP_Model *o2, int b2, int flag)
{
// first thing, see if we're overlapping
res->num_bv_tests++;
if (!BV_Overlap(R, T, o1->child(b1), o2->child(b2))) return;
// if we are, see if we test triangles next
int l1 = o1->child(b1)->Leaf();
int l2 = o2->child(b2)->Leaf();
if (l1 && l2)
{
res->num_tri_tests++;
#if 1
// transform the points in b2 into space of b1, then compare
Tri *t1 = &o1->tris[-o1->child(b1)->first_child - 1];
Tri *t2 = &o2->tris[-o2->child(b2)->first_child - 1];
PQP_REAL q1[3], q2[3], q3[3];
PQP_REAL *p1 = t1->p1;
PQP_REAL *p2 = t1->p2;
PQP_REAL *p3 = t1->p3;
MxVpV(q1, res->R, t2->p1, res->T);
MxVpV(q2, res->R, t2->p2, res->T);
MxVpV(q3, res->R, t2->p3, res->T);
if (TriContact(p1, p2, p3, q1, q2, q3))
{
// add this to result
res->Add(t1->id, t2->id);
}
#else
PQP_REAL p[3], q[3];
Tri *t1 = &o1->tris[-o1->child(b1)->first_child - 1];
Tri *t2 = &o2->tris[-o2->child(b2)->first_child - 1];
if (TriDistance(res->R,res->T,t1,t2,p,q) == 0.0)
{
// add this to result
res->Add(t1->id, t2->id);
}
#endif
return;
}
// we dont, so decide whose children to visit next
PQP_REAL sz1 = o1->child(b1)->GetSize();
PQP_REAL sz2 = o2->child(b2)->GetSize();
PQP_REAL Rc[3][3],Tc[3],Ttemp[3];
if (l2 || (!l1 && (sz1 > sz2)))
{
int c1 = o1->child(b1)->first_child;
int c2 = c1 + 1;
MTxM(Rc,o1->child(c1)->R,R);
#if PQP_BV_TYPE & OBB_TYPE
VmV(Ttemp,T,o1->child(c1)->To);
#else
VmV(Ttemp,T,o1->child(c1)->Tr);
#endif
MTxV(Tc,o1->child(c1)->R,Ttemp);
CollideRecurse(res,Rc,Tc,o1,c1,o2,b2,flag);
if ((flag == PQP_FIRST_CONTACT) && (res->num_pairs > 0)) return;
MTxM(Rc,o1->child(c2)->R,R);
#if PQP_BV_TYPE & OBB_TYPE
VmV(Ttemp,T,o1->child(c2)->To);
#else
VmV(Ttemp,T,o1->child(c2)->Tr);
#endif
MTxV(Tc,o1->child(c2)->R,Ttemp);
CollideRecurse(res,Rc,Tc,o1,c2,o2,b2,flag);
}
else
{
int c1 = o2->child(b2)->first_child;
int c2 = c1 + 1;
MxM(Rc,R,o2->child(c1)->R);
#if PQP_BV_TYPE & OBB_TYPE
MxVpV(Tc,R,o2->child(c1)->To,T);
#else
MxVpV(Tc,R,o2->child(c1)->Tr,T);
#endif
CollideRecurse(res,Rc,Tc,o1,b1,o2,c1,flag);
if ((flag == PQP_FIRST_CONTACT) && (res->num_pairs > 0)) return;
MxM(Rc,R,o2->child(c2)->R);
#if PQP_BV_TYPE & OBB_TYPE
MxVpV(Tc,R,o2->child(c2)->To,T);
#else
MxVpV(Tc,R,o2->child(c2)->Tr,T);
#endif
CollideRecurse(res,Rc,Tc,o1,b1,o2,c2,flag);
}
}
int
PQP_Tolerance(PQP_ToleranceResult *res,
PQP_REAL R1[3][3], PQP_REAL T1[3], PQP_Model *o1,
PQP_REAL R2[3][3], PQP_REAL T2[3], PQP_Model *o2,
PQP_REAL tolerance,
int qsize)
{
double time1 = GetTime();
// make sure that the models are built
if (o1->build_state != PQP_BUILD_STATE_PROCESSED)
return PQP_ERR_UNPROCESSED_MODEL;
if (o2->build_state != PQP_BUILD_STATE_PROCESSED)
return PQP_ERR_UNPROCESSED_MODEL;
// Compute the transform [R,T] that takes us from cs2 to cs1.
// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
MTxM(res->R,R1,R2);
PQP_REAL Ttemp[3];
VmV(Ttemp, T2, T1);
MTxV(res->T, R1, Ttemp);
// set tolerance, used to prune the search
if (tolerance < 0.0) tolerance = 0.0;
res->tolerance = tolerance;
// clear the stats
res->num_bv_tests = 0;
res->num_tri_tests = 0;
// initially assume not closer than tolerance
res->closer_than_tolerance = 0;
// compute the transform from o1->child(0) to o2->child(0)
PQP_REAL Rtemp[3][3], R[3][3], T[3];
MxM(Rtemp,res->R,o2->child(0)->R);
MTxM(R,o1->child(0)->R,Rtemp);
#if PQP_BV_TYPE & RSS_TYPE
MxVpV(Ttemp,res->R,o2->child(0)->Tr,res->T);
VmV(Ttemp,Ttemp,o1->child(0)->Tr);
#else
MxVpV(Ttemp,res->R,o2->child(0)->To,res->T);
VmV(Ttemp,Ttemp,o1->child(0)->To);
#endif
MTxV(T,o1->child(0)->R,Ttemp);
// find a distance lower bound for trivial reject
PQP_REAL d = BV_Distance(R, T, o1->child(0), o2->child(0));
if (d <= res->tolerance)
{
// more work needed - choose routine according to queue size
if (qsize <= 2)
{
ToleranceRecurse(res, R, T, o1, 0, o2, 0);
}
else
{
res->qsize = qsize;
ToleranceQueueRecurse(res, R, T, o1, 0, o2, 0);
}
}
// res->p2 is in cs 1 ; transform it to cs 2
PQP_REAL u[3];
VmV(u, res->p2, res->T);
MTxV(res->p2, res->R, u);
double time2 = GetTime();
res->query_time_secs = time2 - time1;
return PQP_OK;
}
void
ToleranceQueueRecurse(PQP_ToleranceResult *res,
PQP_REAL R[3][3], PQP_REAL T[3],
PQP_Model *o1, int b1,
PQP_Model *o2, int b2)
{
BVTQ bvtq(res->qsize);
BVT min_test;
min_test.b1 = b1;
min_test.b2 = b2;
McM(min_test.R,R);
VcV(min_test.T,T);
while(1)
{
int l1 = o1->child(min_test.b1)->Leaf();
int l2 = o2->child(min_test.b2)->Leaf();
if (l1 && l2)
{
// both leaves - find if tri pair within tolerance
res->num_tri_tests++;
PQP_REAL p[3], q[3];
Tri *t1 = &o1->tris[-o1->child(min_test.b1)->first_child - 1];
Tri *t2 = &o2->tris[-o2->child(min_test.b2)->first_child - 1];
PQP_REAL d = TriDistance(res->R,res->T,t1,t2,p,q);
if (d <= res->tolerance)
{
// triangle pair distance less than tolerance
res->closer_than_tolerance = 1;
res->distance = d;
VcV(res->p1, p); // p already in c.s. 1
VcV(res->p2, q); // q must be transformed
// into c.s. 2 later
return;
}
}
else if (bvtq.GetNumTests() == bvtq.GetSize() - 1)
{
// queue can't get two more tests, recur
ToleranceQueueRecurse(res,min_test.R,min_test.T,
o1,min_test.b1,o2,min_test.b2);
if (res->closer_than_tolerance == 1) return;
}
else
{
// decide how to descend to children
PQP_REAL sz1 = o1->child(min_test.b1)->GetSize();
PQP_REAL sz2 = o2->child(min_test.b2)->GetSize();
res->num_bv_tests += 2;
BVT bvt1,bvt2;
PQP_REAL Ttemp[3];
if (l2 || (!l1 && (sz1 > sz2)))
{
// add two new tests to queue, consisting of min_test.b2
// with the children of min_test.b1
int c1 = o1->child(min_test.b1)->first_child;
int c2 = c1 + 1;
// init bv test 1
bvt1.b1 = c1;
bvt1.b2 = min_test.b2;
MTxM(bvt1.R,o1->child(c1)->R,min_test.R);
#if PQP_BV_TYPE & RSS_TYPE
VmV(Ttemp,min_test.T,o1->child(c1)->Tr);
#else
VmV(Ttemp,min_test.T,o1->child(c1)->To);
#endif
MTxV(bvt1.T,o1->child(c1)->R,Ttemp);
bvt1.d = BV_Distance(bvt1.R,bvt1.T,
o1->child(bvt1.b1),o2->child(bvt1.b2));
// init bv test 2
bvt2.b1 = c2;
bvt2.b2 = min_test.b2;
MTxM(bvt2.R,o1->child(c2)->R,min_test.R);
#if PQP_BV_TYPE & RSS_TYPE
VmV(Ttemp,min_test.T,o1->child(c2)->Tr);
#else
VmV(Ttemp,min_test.T,o1->child(c2)->To);
#endif
MTxV(bvt2.T,o1->child(c2)->R,Ttemp);
bvt2.d = BV_Distance(bvt2.R,bvt2.T,
o1->child(bvt2.b1),o2->child(bvt2.b2));
}
else
{
// add two new tests to queue, consisting of min_test.b1
// with the children of min_test.b2
int c1 = o2->child(min_test.b2)->first_child;
int c2 = c1 + 1;
// init bv test 1
bvt1.b1 = min_test.b1;
bvt1.b2 = c1;
MxM(bvt1.R,min_test.R,o2->child(c1)->R);
#if PQP_BV_TYPE & RSS_TYPE
MxVpV(bvt1.T,min_test.R,o2->child(c1)->Tr,min_test.T);
#else
MxVpV(bvt1.T,min_test.R,o2->child(c1)->To,min_test.T);
#endif
bvt1.d = BV_Distance(bvt1.R,bvt1.T,
o1->child(bvt1.b1),o2->child(bvt1.b2));
// init bv test 2
bvt2.b1 = min_test.b1;
bvt2.b2 = c2;
MxM(bvt2.R,min_test.R,o2->child(c2)->R);
#if PQP_BV_TYPE & RSS_TYPE
MxVpV(bvt2.T,min_test.R,o2->child(c2)->Tr,min_test.T);
#else
MxVpV(bvt2.T,min_test.R,o2->child(c2)->To,min_test.T);
#endif
bvt2.d = BV_Distance(bvt2.R,bvt2.T,
o1->child(bvt2.b1),o2->child(bvt2.b2));
}
// put children tests in queue
if (bvt1.d <= res->tolerance) bvtq.AddTest(bvt1);
if (bvt2.d <= res->tolerance) bvtq.AddTest(bvt2);
}
if (bvtq.Empty() || (bvtq.MinTest() > res->tolerance))
{
res->closer_than_tolerance = 0;
return;
}
else
{
min_test = bvtq.ExtractMinTest();
}
}
}
// Tolerance Stuff
//
//---------------------------------------------------------------------------
void
ToleranceRecurse(PQP_ToleranceResult *res,
PQP_REAL R[3][3], PQP_REAL T[3],
PQP_Model *o1, int b1, PQP_Model *o2, int b2)
{
PQP_REAL sz1 = o1->child(b1)->GetSize();
PQP_REAL sz2 = o2->child(b2)->GetSize();
int l1 = o1->child(b1)->Leaf();
int l2 = o2->child(b2)->Leaf();
if (l1 && l2)
{
// both leaves - find if tri pair within tolerance
res->num_tri_tests++;
PQP_REAL p[3], q[3];
Tri *t1 = &o1->tris[-o1->child(b1)->first_child - 1];
Tri *t2 = &o2->tris[-o2->child(b2)->first_child - 1];
PQP_REAL d = TriDistance(res->R,res->T,t1,t2,p,q);
if (d <= res->tolerance)
{
// triangle pair distance less than tolerance
res->closer_than_tolerance = 1;
res->distance = d;
VcV(res->p1, p); // p already in c.s. 1
VcV(res->p2, q); // q must be transformed
// into c.s. 2 later
}
return;
}
int a1,a2,c1,c2; // new bv tests 'a' and 'c'
PQP_REAL R1[3][3], T1[3], R2[3][3], T2[3], Ttemp[3];
if (l2 || (!l1 && (sz1 > sz2)))
{
// visit the children of b1
a1 = o1->child(b1)->first_child;
a2 = b2;
c1 = o1->child(b1)->first_child+1;
c2 = b2;
MTxM(R1,o1->child(a1)->R,R);
#if PQP_BV_TYPE & RSS_TYPE
VmV(Ttemp,T,o1->child(a1)->Tr);
#else
VmV(Ttemp,T,o1->child(a1)->To);
#endif
MTxV(T1,o1->child(a1)->R,Ttemp);
MTxM(R2,o1->child(c1)->R,R);
#if PQP_BV_TYPE & RSS_TYPE
VmV(Ttemp,T,o1->child(c1)->Tr);
#else
VmV(Ttemp,T,o1->child(c1)->To);
#endif
MTxV(T2,o1->child(c1)->R,Ttemp);
}
else
{
// visit the children of b2
a1 = b1;
a2 = o2->child(b2)->first_child;
c1 = b1;
c2 = o2->child(b2)->first_child+1;
MxM(R1,R,o2->child(a2)->R);
#if PQP_BV_TYPE & RSS_TYPE
MxVpV(T1,R,o2->child(a2)->Tr,T);
#else
MxVpV(T1,R,o2->child(a2)->To,T);
#endif
MxM(R2,R,o2->child(c2)->R);
#if PQP_BV_TYPE & RSS_TYPE
MxVpV(T2,R,o2->child(c2)->Tr,T);
#else
MxVpV(T2,R,o2->child(c2)->To,T);
#endif
}
res->num_bv_tests += 2;
PQP_REAL d1 = BV_Distance(R1, T1, o1->child(a1), o2->child(a2));
PQP_REAL d2 = BV_Distance(R2, T2, o1->child(c1), o2->child(c2));
if (d2 < d1)
{
if (d2 <= res->tolerance) ToleranceRecurse(res, R2, T2, o1, c1, o2, c2);
if (res->closer_than_tolerance) return;
if (d1 <= res->tolerance) ToleranceRecurse(res, R1, T1, o1, a1, o2, a2);
}
else
{
if (d1 <= res->tolerance) ToleranceRecurse(res, R1, T1, o1, a1, o2, a2);
if (res->closer_than_tolerance) return;
if (d2 <= res->tolerance) ToleranceRecurse(res, R2, T2, o1, c1, o2, c2);
}
}
void evaluate(Matrix u, const Matrix v, void* ctx)
{
poisson_info_t* info = ctx;
MxM(u, info->A, v, 1.0, 0.0, 'N', 'N');
MxM(u, v, info->A, 1.0, 1.0, 'N', 'N');
}
void
DisplayCB()
{
BeginDraw();
// set up model transformations
if (animate)
{
rot1 += .1;
rot2 += .2;
rot3 += .3;
}
PQP_REAL R1[3][3],R2[3][3],T1[3],T2[3];
PQP_REAL M1[3][3],M2[3][3],M3[3][3];
T1[0] = -1;
T1[1] = 0.0;
T1[2] = 0.0;
T2[0] = 1;
T2[1] = 0.0;
T2[2] = 0.0;
MRotX(M1,rot1);
MRotY(M2,rot2);
MxM(M3,M1,M2);
MRotZ(M1,rot3);
MxM(R1,M3,M1);
MRotX(M1,rot3);
MRotY(M2,rot1);
MxM(M3,M1,M2);
MRotZ(M1,rot2);
MxM(R2,M3,M1);
// perform distance query
PQP_REAL rel_err = 0.0;
PQP_REAL abs_err = 0.0;
PQP_DistanceResult res;
PQP_Distance(&res,R1,T1,&bunny,R2,T2,&torus,rel_err,abs_err);
// draw the models
glColor3d(0.0,0.0,1.0);
double oglm[16];
MVtoOGL(oglm,R1,T1);
glPushMatrix();
glMultMatrixd(oglm);
bunny_to_draw->Draw();
glPopMatrix();
glColor3d(0.0,1.0,0.0);
MVtoOGL(oglm,R2,T2);
glPushMatrix();
glMultMatrixd(oglm);
torus_to_draw->Draw();
glPopMatrix();
// draw the closest points as small spheres
glColor3d(1.0,0.0,0.0);
PQP_REAL P1[3],P2[3],V1[3],V2[3];
VcV(P1,res.P1());
VcV(P2,res.P2());
// each point is in the space of its model;
// transform to world space
MxVpV(V1,R1,P1,T1);
/*
glPushMatrix();
glTranslated(V1[0],V1[1],V1[2]);
glutSolidSphere(.05,15,15);
glPopMatrix();
*/
MxVpV(V2,R2,P2,T2);
/*
glPushMatrix();
glTranslated(V2[0],V2[1],V2[2]);
glutSolidSphere(.05,15,15);
glPopMatrix();
*/
// draw the line between the closest points
float v1p[3], v2p[3];
for (int i = 0; i < 3; ++i) {
v1p[i] = V1[i];
v2p[i] = V2[i];
}
//glDisable(GL_LIGHTING);
glBegin(GL_LINES);
glVertex3v(v1p);
glVertex3v(v2p);
glEnd();
//glEnable(GL_LIGHTING);
EndDraw();
}
