static btVector3 convexHullSupport (const btVector3& localDirOrg, const btVector3* points, int numPoints, const btVector3& localScaling)
{
btVector3 vec = localDirOrg * localScaling;
#if defined (__CELLOS_LV2__) && defined (__SPU__)
btVector3 localDir = vec;
vec_float4 v_distMax = {-FLT_MAX,0,0,0};
vec_int4 v_idxMax = {-999,0,0,0};
int v=0;
int numverts = numPoints;
for(;v<(int)numverts-4;v+=4) {
vec_float4 p0 = vec_dot3(points[v ].get128(),localDir.get128());
vec_float4 p1 = vec_dot3(points[v+1].get128(),localDir.get128());
vec_float4 p2 = vec_dot3(points[v+2].get128(),localDir.get128());
vec_float4 p3 = vec_dot3(points[v+3].get128(),localDir.get128());
const vec_int4 i0 = {v ,0,0,0};
const vec_int4 i1 = {v+1,0,0,0};
const vec_int4 i2 = {v+2,0,0,0};
const vec_int4 i3 = {v+3,0,0,0};
vec_uint4 retGt01 = spu_cmpgt(p0,p1);
vec_float4 pmax01 = spu_sel(p1,p0,retGt01);
vec_int4 imax01 = spu_sel(i1,i0,retGt01);
vec_uint4 retGt23 = spu_cmpgt(p2,p3);
vec_float4 pmax23 = spu_sel(p3,p2,retGt23);
vec_int4 imax23 = spu_sel(i3,i2,retGt23);
vec_uint4 retGt0123 = spu_cmpgt(pmax01,pmax23);
vec_float4 pmax0123 = spu_sel(pmax23,pmax01,retGt0123);
vec_int4 imax0123 = spu_sel(imax23,imax01,retGt0123);
vec_uint4 retGtMax = spu_cmpgt(v_distMax,pmax0123);
v_distMax = spu_sel(pmax0123,v_distMax,retGtMax);
v_idxMax = spu_sel(imax0123,v_idxMax,retGtMax);
}
for(;v<(int)numverts;v++) {
vec_float4 p = vec_dot3(points[v].get128(),localDir.get128());
const vec_int4 i = {v,0,0,0};
vec_uint4 retGtMax = spu_cmpgt(v_distMax,p);
v_distMax = spu_sel(p,v_distMax,retGtMax);
v_idxMax = spu_sel(i,v_idxMax,retGtMax);
}
int ptIndex = spu_extract(v_idxMax,0);
const btVector3& supVec= points[ptIndex] * localScaling;
return supVec;
#else
btScalar maxDot;
long ptIndex = vec.maxDot( points, numPoints, maxDot);
btAssert(ptIndex >= 0);
btVector3 supVec = points[ptIndex] * localScaling;
return supVec;
#endif //__SPU__
}
/*
* Perform a linear transform in four dimensional space
* by applying a 3 x 3 matrix to a 3 x 1 column vector.
*
* PARAMETERS:
* y - transform matrix
* x - input vector
* z - output vector
*/
void xform3(double y[][3], double *x, double *z) {
double *temp = (double *)alloca(sizeof(double) * 3);
*(temp + 0) = vec_dot3(y[0], x);
*(temp + 1) = vec_dot3(y[1], x);
*(temp + 2) = vec_dot3(y[2], x);
*(z + 0) = *(temp + 0);
*(z + 1) = *(temp + 1);
*(z + 2) = *(temp + 2);
}
/*
* Projct a vector onto a plane
*
* PARAMETERS:
* normal - plane normal
* in - input vector
* proj - projected vector
*/
void vec_project3(double *normal, double *in, double *proj) {
double temp[3];
double NdotIn = vec_dot3(normal, in);
vec_scale3(NdotIn, normal, temp);
vec_diff3(temp, in, proj);
}
/*
* Finds the direction that is a reflection of unitin across unitnorm.
*
* PARAMETERS:
* unitin - unit vector input direction
* unitnorm - unit vector normal
* unitout - unit vector output direction
*/
void vec_reflect3(double *unitin, double *unitnorm, double *unitout) {
double temp[3];
double UdotN;
double U[3];
vec_scale3(-1, unitin, U);
UdotN = (vec_dot3(U, unitnorm));
vec_scale3(UdotN, unitnorm, temp);
vec_scale3(2, temp, temp);
vec_diff3(U, temp, unitout);
}
/*
* 3 x 3 matrix multiplier.
*
* PARAMETERS:
* x - left input vector
* y - right input vector
* out - output matrix
*/
void vec_matmul3(double x[][3], double y[][3],
double out[][3]) {
int i;
int j;
double temp[3];
for(i = 0; i < 3; i++) {
for(j=0; j < 3; j++) {
temp[0] = y[0][j];
temp[1] = y[1][j];
temp[2] = y[2][j];
out[i][j] = vec_dot3(x[i], temp);
}
}
}
/**
* Tests veclib3d by running tests against each method and checking the
* result of the method calls against assertions.
*
* RETURN:
* Return success if all tests pass, exit with failure otherwise
*/
int main() {
double v1[3] = {1, 2, 3};
double v2[3] = {0, -5, 9};
double v3[3];
double v4;
// test vec_prn3
printf("Testing vectors:\n");
vec_prn3("v1", v1);
vec_prn3("v2", v2);
// test vec_dot3
printf("testing vec_dot3...\n");
printf("\ttesting normal operation: v1 dot v2: ");
v4 = vec_dot3(v1, v2);
assert(v4 == 17);
printf("good\n");
printf("\ttesting aliasing: v1 dot v1: ");
v4 = vec_dot3(v1, v1);
assert(v4 == 14);
printf("good\n");
// test vec_scale3
printf("testing vec_scale3...\n");
printf("\ttesting normal operation: 10 * v1 -> v3: ");
vec_scale3(10, v1, v3);
assert(v3[0] == 10 && v3[1] == 20 && v3[2] == 30);
printf("good\n");
printf("\ttesting aliasing: 10 * v1 -> v1: ");
vec_scale3(10, v1, v1);
assert(v1[0] == 10 && v1[1] == 20 && v1[2] == 30);
v1[0] = 1;
v1[1] = 2;
v1[2] = 3;
printf("good\n");
printf("\ttesting zero factor: 0 * v1 -> v3: ");
vec_scale3(0, v1, v3);
assert(v3[0] == 0 && v3[1] == 0 && v3[2] == 0);
printf("good\n");
printf("\ttesting negative factor: -1 * v1 -> v3: ");
vec_scale3(-1, v1, v3);
assert(v3[0] == (-1 * v1[0]) &&
v3[1] == (-1 * v1[1]) &&
v3[2] == (-1 * v1[2]));
printf("good\n");
// test vec_length3
printf("testing vec_length3...\n");
printf("\ttesting normal operation: ||v1||: ");
v4 = vec_length3(v1);
assert(v4 == sqrt(14));
printf("good\n");
printf("\ttesting zero vector: ||{0, 0, 0}||: ");
v3[0] = v3[1] = v3[2] = 0;
v4 = vec_length3(v3);
assert(v4 == 0);
printf("good\n");
printf("\ttesting negative vector: ||v2||: ");
v4 = vec_length3(v2);
assert(v4 == sqrt(106));
printf("good\n");
// test vec_diff3
printf("testing vec_diff3...\n");
printf("\ttesting normal operation: v1 - v2 -> v3: ");
vec_diff3(v2, v1, v3);
assert(v3[0] == 1 && v3[1] == 7 && v3[2] == -6);
printf("good\n");
printf("\ttesting aliasing 1: v1 - v2 -> v1: ");
vec_diff3(v2, v1, v1);
assert(v1[0] == 1 && v1[1] == 7 && v1[2] == -6);
v1[0] = 1;
v1[1] = 2;
v1[2] = 3;
printf("good\n");
printf("\ttesting aliasing 2: v1 - v2 -> v2: ");
vec_diff3(v2, v1, v2);
assert(v2[0] == 1 && v2[1] == 7 && v2[2] == -6);
v2[0] = 0;
v2[1] = -5;
v2[2] = 9;
printf("good\n");
// test vec_sum3
printf("testing vec_sum3...\n");
printf("\ttesting normal operation: v1 + v2 -> v3: ");
vec_sum3(v2, v1, v3);
assert(v3[0] == 1 && v3[1] == -3 && v3[2] == 12);
printf("good\n");
printf("\ttesting aliasing 1: v1 + v2 -> v1: ");
vec_sum3(v2, v1, v1);
assert(v1[0] == 1 && v1[1] == -3 && v1[2] == 12);
v1[0] = 1;
v1[1] = 2;
v1[2] = 3;
printf("good\n");
printf("\ttesting aliasing 2: v1 + v2 -> v2: ");
vec_sum3(v2, v1, v2);
assert(v2[0] == 1 && v2[1] == -3 && v2[2] == 12);
v2[0] = 0;
v2[1] = -5;
v2[2] = 9;
printf("good\n");
// test vec_unit3
printf("testing vec_unit3...\n");
printf("\ttesting normal operation: v1 -> v3: ");
vec_unit3(v1, v3);
assert(vec_length3(v3) == 1);
printf("good\n");
printf("\ttesting aliasing: v1 -> v1: ");
vec_unit3(v1, v1);
assert(vec_length3(v1) == 1);
v1[0] = 1;
v1[1] = 2;
v1[2] = 3;
printf("good\n");
printf("\ttesting zero vector: {0, 0, 0} -> v3: ");
v3[0] = v3[1] = v3[2] = 0;
vec_unit3(v3, v3);
assert(vec_length3(v3) == 0);
printf("good\n");
return 0;
}