BVH_REAL TriangleDistance::triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
const Vec3f R[3], const Vec3f& Tl,
Vec3f& P, Vec3f& Q)
{
Vec3f T1_transformed = MxV(R, T1) + Tl;
Vec3f T2_transformed = MxV(R, T2) + Tl;
Vec3f T3_transformed = MxV(R, T3) + Tl;
return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q);
}
void DiagonalizationPoisson1D(Vector u, const Vector lambda, const Matrix Q)
{
Vector btilde = createVector(u->len);
int i;
MxV(btilde, Q, u, 1.0, 0.0, 'T');
for (i=0;i<btilde->len;++i)
btilde->data[i] /= lambda->data[i];
MxV(u, Q, btilde, 1.0, 0.0, 'N');
freeVector(btilde);
}
BVH_REAL TriangleDistance::triDistance(const Vec3f S[3], const Vec3f T[3],
const Vec3f R[3], const Vec3f& Tl,
Vec3f& P, Vec3f& Q)
{
Vec3f T_transformed[3];
T_transformed[0] = MxV(R, T[0]) + Tl;
T_transformed[1] = MxV(R, T[1]) + Tl;
T_transformed[2] = MxV(R, T[2]) + Tl;
return triDistance(S, T_transformed, P, Q);
}
int cg(Matrix A, Vector b, double tolerance)
{
int i=0, j;
double rl;
Vector r = createVector(b->len);
Vector p = createVector(b->len);
Vector buffer = createVector(b->len);
double dotp = 1000;
double rdr = dotp;
copyVector(r,b);
fillVector(b, 0.0);
rl = sqrt(dotproduct(r,r));
while (i < b->len && rdr > tolerance*rl) {
++i;
if (i == 1) {
copyVector(p,r);
dotp = dotproduct(r,r);
} else {
double dotp2 = dotproduct(r,r);
double beta = dotp2/dotp;
dotp = dotp2;
scaleVector(p,beta);
axpy(p,r,1.0);
}
MxV(buffer, p);
double alpha = dotp/dotproduct(p,buffer);
axpy(b,p,alpha);
axpy(r,buffer,-alpha);
rdr = sqrt(dotproduct(r,r));
}
freeVector(r);
freeVector(p);
freeVector(buffer);
return i;
}
double dosum(double** A, double** v, int K, int N)
{
double alpha=0;
double temp[N];
for( int i=0;i<K;++i ) {
MxV(temp,A,v[i],N);
alpha += innerproduct(temp,v[i],N);
}
return alpha;
}
// perform a matrix-vector product
void myMxV(Vector u, Matrix A, Vector v)
{
Vector temp = createVector(A->rows);
MxV(temp, A, v);
#ifdef HAVE_MPI
for (int i=0;i<v->comm_size;++i)
MPI_Reduce(temp->data+v->displ[i], u->data, v->sizes[i],
MPI_DOUBLE, MPI_SUM, i, *v->comm);
#else
memcpy(u->data, temp->data, u->len*sizeof(double));
#endif
freeVector(temp);
}
double dosum(double** A, double** v, int K, int N)
{
double alpha=0;
/* CHANGED */
double** temp = createMatrix(K,N);
for( int i=0; i<K; ++i ) {
/* CHANGED */
MxV(temp[i],A,v[i],N);
alpha += innerproduct(temp[i],v[i],N);
}
return alpha;
}
double dosum(double** A, double** v, int K, int N)
{
double alpha=0;
double** temp = createMatrix(K,N);
#pragma omp parallel for schedule(static) \
reduction(+:alpha)
for( int i=0;i<K;++i ) {
MxV(temp[i],A,v[i],N);
alpha += innerproduct(temp[i],v[i],N);
}
return alpha;
}
// calculates \sum_K v_i'*A*v_i
double dosum(Matrix A, Matrix v)
{
double alpha=0.0;
Matrix temp;
int i, t;
t = getMaxThreads();
temp = createMatrix(A->rows, t);
#pragma omp parallel for schedule(static) reduction(+:alpha)
for(i=0;i<v->cols;++i) {
MxV(temp->col[getCurrentThread()],A,v->col[i], 1.0, 0.0, 'N');
alpha += dotproduct(temp->col[getCurrentThread()],v->col[i]);
}
freeMatrix(temp);
return alpha;
}
int extGetNextRecord(double *in, int numIn, double *out, int numOut)
{
double inrtl_body[3][3];
/* Set initial matrix with input from SIM */
inrtl_body[0][0] = in[3];
inrtl_body[1][0] = in[4];
inrtl_body[2][0] = in[5];
inrtl_body[0][1] = in[6];
inrtl_body[1][1] = in[7];
inrtl_body[2][1] = in[8];
inrtl_body[0][2] = in[9];
inrtl_body[1][2] = in[10];
inrtl_body[2][2] = in[11];
/* Perform matrix function */
MxV(out, inrtl_body, in);
return (0);
}
void
BV::FitToTris(PQP_REAL O[3][3], Tri *tris, int num_tris)
{
// store orientation
McM(R,O);
// project points of tris to R coordinates
int num_points = 3*num_tris;
PQP_REAL (*P)[3] = new PQP_REAL[num_points][3];
int point = 0;
int i;
for (i = 0; i < num_tris; i++)
{
MTxV(P[point],R,tris[i].p1);
point++;
MTxV(P[point],R,tris[i].p2);
point++;
MTxV(P[point],R,tris[i].p3);
point++;
}
PQP_REAL minx, maxx, miny, maxy, minz, maxz, c[3];
#if PQP_BV_TYPE & OBB_TYPE
minx = maxx = P[0][0];
miny = maxy = P[0][1];
minz = maxz = P[0][2];
for (i = 1; i < num_points; i++)
{
if (P[i][0] < minx) minx = P[i][0];
else if (P[i][0] > maxx) maxx = P[i][0];
if (P[i][1] < miny) miny = P[i][1];
else if (P[i][1] > maxy) maxy = P[i][1];
if (P[i][2] < minz) minz = P[i][2];
else if (P[i][2] > maxz) maxz = P[i][2];
}
c[0] = (PQP_REAL)0.5*(maxx + minx);
c[1] = (PQP_REAL)0.5*(maxy + miny);
c[2] = (PQP_REAL)0.5*(maxz + minz);
MxV(To,R,c);
d[0] = (PQP_REAL)0.5*(maxx - minx);
d[1] = (PQP_REAL)0.5*(maxy - miny);
d[2] = (PQP_REAL)0.5*(maxz - minz);
#endif
#if PQP_BV_TYPE & RSS_TYPE
// compute thickness, which determines radius, and z of rectangle corner
PQP_REAL cz,radsqr;
minz = maxz = P[0][2];
for (i = 1; i < num_points; i++)
{
if (P[i][2] < minz) minz = P[i][2];
else if (P[i][2] > maxz) maxz = P[i][2];
}
r = (PQP_REAL)0.5*(maxz - minz);
radsqr = r*r;
cz = (PQP_REAL)0.5*(maxz + minz);
// compute an initial length of rectangle along x direction
// find minx and maxx as starting points
int minindex, maxindex;
minindex = maxindex = 0;
for (i = 1; i < num_points; i++)
{
if (P[i][0] < P[minindex][0]) minindex = i;
else if (P[i][0] > P[maxindex][0]) maxindex = i;
}
PQP_REAL x, dz;
dz = P[minindex][2] - cz;
minx = P[minindex][0] + sqrt(MaxOfTwo(radsqr - dz*dz,0));
dz = P[maxindex][2] - cz;
maxx = P[maxindex][0] - sqrt(MaxOfTwo(radsqr - dz*dz,0));
// grow minx
for (i = 0; i < num_points; i++)
{
if (P[i][0] < minx)
{
dz = P[i][2] - cz;
x = P[i][0] + sqrt(MaxOfTwo(radsqr - dz*dz,0));
if (x < minx) minx = x;
}
}
// grow maxx
for (i = 0; i < num_points; i++)
{
if (P[i][0] > maxx)
{
dz = P[i][2] - cz;
x = P[i][0] - sqrt(MaxOfTwo(radsqr - dz*dz,0));
if (x > maxx) maxx = x;
}
}
// compute an initial length of rectangle along y direction
// find miny and maxy as starting points
minindex = maxindex = 0;
for (i = 1; i < num_points; i++)
{
if (P[i][1] < P[minindex][1]) minindex = i;
else if (P[i][1] > P[maxindex][1]) maxindex = i;
}
PQP_REAL y;
dz = P[minindex][2] - cz;
miny = P[minindex][1] + sqrt(MaxOfTwo(radsqr - dz*dz,0));
dz = P[maxindex][2] - cz;
maxy = P[maxindex][1] - sqrt(MaxOfTwo(radsqr - dz*dz,0));
// grow miny
for (i = 0; i < num_points; i++)
{
if (P[i][1] < miny)
{
dz = P[i][2] - cz;
y = P[i][1] + sqrt(MaxOfTwo(radsqr - dz*dz,0));
if (y < miny) miny = y;
}
}
// grow maxy
for (i = 0; i < num_points; i++)
{
if (P[i][1] > maxy)
{
dz = P[i][2] - cz;
y = P[i][1] - sqrt(MaxOfTwo(radsqr - dz*dz,0));
if (y > maxy) maxy = y;
}
}
// corners may have some points which are not covered - grow lengths if
// necessary
PQP_REAL dx, dy, u, t;
PQP_REAL a = sqrt((PQP_REAL)0.5);
for (i = 0; i < num_points; i++)
{
if (P[i][0] > maxx)
{
if (P[i][1] > maxy)
{
dx = P[i][0] - maxx;
dy = P[i][1] - maxy;
u = dx*a + dy*a;
t = (a*u - dx)*(a*u - dx) +
(a*u - dy)*(a*u - dy) +
(cz - P[i][2])*(cz - P[i][2]);
u = u - sqrt(MaxOfTwo(radsqr - t,0));
if (u > 0)
{
maxx += u*a;
maxy += u*a;
}
}
else if (P[i][1] < miny)
{
dx = P[i][0] - maxx;
dy = P[i][1] - miny;
u = dx*a - dy*a;
t = (a*u - dx)*(a*u - dx) +
(-a*u - dy)*(-a*u - dy) +
(cz - P[i][2])*(cz - P[i][2]);
u = u - sqrt(MaxOfTwo(radsqr - t,0));
if (u > 0)
{
maxx += u*a;
miny -= u*a;
}
}
}
else if (P[i][0] < minx)
{
if (P[i][1] > maxy)
{
dx = P[i][0] - minx;
dy = P[i][1] - maxy;
u = dy*a - dx*a;
t = (-a*u - dx)*(-a*u - dx) +
(a*u - dy)*(a*u - dy) +
(cz - P[i][2])*(cz - P[i][2]);
u = u - sqrt(MaxOfTwo(radsqr - t,0));
if (u > 0)
{
minx -= u*a;
maxy += u*a;
}
}
else if (P[i][1] < miny)
{
dx = P[i][0] - minx;
dy = P[i][1] - miny;
u = -dx*a - dy*a;
t = (-a*u - dx)*(-a*u - dx) +
(-a*u - dy)*(-a*u - dy) +
(cz - P[i][2])*(cz - P[i][2]);
u = u - sqrt(MaxOfTwo(radsqr - t,0));
if (u > 0)
{
minx -= u*a;
miny -= u*a;
}
}
}
}
c[0] = minx;
c[1] = miny;
c[2] = cz;
MxV(Tr,R,c);
l[0] = maxx - minx;
if (l[0] < 0) l[0] = 0;
l[1] = maxy - miny;
if (l[1] < 0) l[1] = 0;
#endif
delete [] P;
}