void BI_cvvamn2(int N, char *vec1, char *vec2)
{
int r, i;
float *v1=(float*)vec1, *v2=(float*)vec2;
float diff;
N *= 2;
for (r=0, i=1; r != N; r += 2, i += 2)
{
diff = (Rabs(v1[r]) + Rabs(v1[i])) - (Rabs(v2[r]) + Rabs(v2[i]));
if (diff > 0)
{
v1[r] = v2[r];
v1[i] = v2[i];
}
else if (diff == 0)
{
if (v1[r] != v2[r])
{
if (v1[r] < v2[r])
{
v1[r] = v2[r];
v1[i] = v2[i];
}
}
else
{
if (v1[i] < v2[i])
{
v1[r] = v2[r];
v1[i] = v2[i];
}
}
}
}
}
void BI_zvvamx2(int N, char *vec1, char *vec2)
{
int r, i;
double *v1=(double*)vec1, *v2=(double*)vec2;
double diff;
N *= 2;
for (r=0, i=1; r != N; r += 2, i += 2)
{
diff = (Rabs(v1[r]) + Rabs(v1[i])) - (Rabs(v2[r]) + Rabs(v2[i]));
if (diff < 0)
{
v1[r] = v2[r];
v1[i] = v2[i];
}
else if (diff == 0)
{
if (v1[r] != v2[r])
{
if (v1[r] < v2[r])
{
v1[r] = v2[r];
v1[i] = v2[i];
}
}
else
{
if (v1[i] < v2[i])
{
v1[r] = v2[r];
v1[i] = v2[i];
}
}
}
}
}
ChNarrowPhaseCollider::eCollSuccess CHAABBcollider::ComputeCollisions(
ChMatrix33<>& R1, Vector T1, ChCollisionTree *oc1,
ChMatrix33<>& R2, Vector T2, ChCollisionTree *oc2,
eCollMode flag)
{
double t1 = GetTime();
// INHERIT parent class behaviour
if (ChNarrowPhaseCollider::ComputeCollisions(
R1, T1, oc1,
R2, T2, oc2,
flag) != ChC_RESULT_OK)
return ChC_RESULT_GENERICERROR;
// Downcasting
CHAABBTree* o1 = (CHAABBTree*)oc1;
CHAABBTree* o2 = (CHAABBTree*)oc2;
// clear the stats
this->num_bv_tests = 0;
this->num_geo_tests = 0;
// Precompute the Rabs matrix, to be used many times in AABB collisions
const double reps = (double)1e-6;
// Rabs = fabs(R)+eps
Rabs(0,0) = myfabs(R.Get33Element(0,0)); Rabs(0,0) += reps;
Rabs(0,1) = myfabs(R.Get33Element(0,1)); Rabs(0,1) += reps;
Rabs(0,2) = myfabs(R.Get33Element(0,2)); Rabs(0,2) += reps;
Rabs(1,0) = myfabs(R.Get33Element(1,0)); Rabs(1,0) += reps;
Rabs(1,1) = myfabs(R.Get33Element(1,1)); Rabs(1,1) += reps;
Rabs(1,2) = myfabs(R.Get33Element(1,2)); Rabs(1,2) += reps;
Rabs(2,0) = myfabs(R.Get33Element(2,0)); Rabs(2,0) += reps;
Rabs(2,1) = myfabs(R.Get33Element(2,1)); Rabs(2,1) += reps;
Rabs(2,2) = myfabs(R.Get33Element(2,2)); Rabs(2,2) += reps;
// Now start with both top level BVs and recurse...
CollideRecurse(o1,0,o2,0,flag);
double t2 = GetTime();
this->query_time_secs = t2 - t1;
return ChC_RESULT_OK;
}