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;
}
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;
}
ChNarrowPhaseCollider::eCollSuccess CHOBBcollider::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
CHOBBTree* o1 = (CHOBBTree*)oc1;
CHOBBTree* o2 = (CHOBBTree*)oc2;
// clear the stats
this->num_bv_tests = 0;
this->num_geo_tests = 0;
// compute the transform from o1->child(0) to o2->child(0)
static ChMatrix33<> Rtemp;
static ChMatrix33<> bR;
static Vector bT;
static Vector Ttemp;
Rtemp.MatrMultiply(this->R,o2->child(0)->Rot); // MxM(Rtemp,this->R,o2->child(0)->R);
bR.MatrMultiply(o1->child(0)->Rot, Rtemp); // MTxM(R,o1->child(0)->R,Rtemp);
Ttemp = ChTrasform<>::TrasformLocalToParent(o2->child(0)->To, this->T, this->R); // MxVpV(Ttemp,this->R,o2->child(0)->To,this->T);
Ttemp = Vsub(Ttemp, o1->child(0)->To); // VmV(Ttemp,Ttemp,o1->child(0)->To);
bT = o1->child(0)->Rot.MatrT_x_Vect(Ttemp); // MTxV(T,o1->child(0)->R,Ttemp);
// now start with both top level BVs
CollideRecurse(bR,bT,o1,0,o2,0,flag);
double t2 = GetTime();
this->query_time_secs = t2 - t1;
return ChC_RESULT_OK;
}
void CHAABBcollider::CollideRecurse(
CHAABBTree *o1, int b1,
CHAABBTree *o2, int b2,
eCollMode flag)
{
CHAABB* box1 = o1->child(b1);
CHAABB* box2 = o2->child(b2);
this->num_bv_tests++;
// first thing, see if we're overlapping
static Vector Translation;
Translation = Vsub (this->T, box1->To);
Translation = Vadd (Translation, this->R.Matr_x_Vect(box2->To));
if (!CHAABB::AABB_Overlap(this->R, this->Rabs, Translation, box1, box2))
return;
// if we are, see if we test triangles next
int l1 = box1->IsLeaf();
int l2 = box2->IsLeaf();
//
// CASE TWO LEAVES
//
if (l1 && l2)
{
this->num_geo_tests++;
// transform the points in b2 into space of b1, then compare
ChGeometry* mgeo1 = o1->geometries[box1->GetGeometryIndex()];
ChGeometry* mgeo2 = o2->geometries[box2->GetGeometryIndex()];
bool just_intersect = false;
if (flag==ChNarrowPhaseCollider::ChC_FIRST_CONTACT)
just_intersect = true;
ChGeometryCollider::ComputeCollisions(*mgeo1, &this->R1, &this->T1,
*mgeo2, &this->R2, &this->T2,
*this,
&this->R, &this->T,
just_intersect);
return;
}
// we dont, so decide whose children to visit next
double sz1 = box1->GetSize();
double sz2 = box2->GetSize();
if (l2 || (!l1 && (sz1 > sz2)))
{
int c1 = box1->GetFirstChildIndex();
int c2 = box1->GetSecondChildIndex();
CollideRecurse(o1,c1,o2,b2,flag);
if ((flag == ChC_FIRST_CONTACT) && (this->GetNumPairs() > 0)) return;
CollideRecurse(o1,c2,o2,b2,flag);
}
else
{
int c1 = box2->GetFirstChildIndex();
int c2 = box2->GetSecondChildIndex();
CollideRecurse(o1,b1,o2,c1,flag);
if ((flag == ChC_FIRST_CONTACT) && (this->GetNumPairs() > 0)) return;
CollideRecurse(o1,b1,o2,c2,flag);
}
}
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);
}
}
void CHOBBcollider::CollideRecurse(
ChMatrix33<>& boR, Vector& boT,
CHOBBTree *o1, int b1,
CHOBBTree *o2, int b2,
eCollMode flag)
{
this->num_bv_tests++;
CHOBB* box1 = o1->child(b1);
CHOBB* box2 = o2->child(b2);
// first thing, see if we're overlapping
if (!CHOBB::OBB_Overlap(boR, boT, box1, box2))
return;
// if we are, see if we test geometries next
int l1 = box1->IsLeaf();
int l2 = box2->IsLeaf();
//
// CASE TWO LEAVES
//
if (l1 && l2)
{
this->num_geo_tests++;
// transform the points in b2 into space of b1, then compare
ChGeometry* mgeo1 = o1->geometries[box1->GetGeometryIndex()];
ChGeometry* mgeo2 = o2->geometries[box2->GetGeometryIndex()];
bool just_intersect = false;
if (flag==ChNarrowPhaseCollider::ChC_FIRST_CONTACT)
just_intersect = true;
ChGeometryCollider::ComputeCollisions(*mgeo1, &this->R1, &this->T1,
*mgeo2, &this->R2, &this->T2,
*this,
&this->R, &this->T,
just_intersect);
return;
}
// we dont, so decide whose children to visit next
double sz1 = box1->GetSize();
double sz2 = box2->GetSize();
ChMatrix33<> Rc;
Vector Tc;
if (l2 || (!l1 && (sz1 > sz2)))
{
int c1 = box1->GetFirstChildIndex();
int c2 = box1->GetSecondChildIndex();
Rc.MatrTMultiply(o1->child(c1)->Rot, boR);
Tc = ChTrasform<>::TrasformParentToLocal(boT, o1->child(c1)->To, o1->child(c1)->Rot);
CollideRecurse(Rc,Tc,o1,c1,o2,b2,flag);
if ((flag == ChC_FIRST_CONTACT) && (this->GetNumPairs() > 0)) return;
Rc.MatrTMultiply(o1->child(c2)->Rot, boR);
Tc = ChTrasform<>::TrasformParentToLocal(boT, o1->child(c2)->To, o1->child(c2)->Rot);
CollideRecurse(Rc,Tc,o1,c2,o2,b2,flag);
}
else
{
int c1 = box2->GetFirstChildIndex();
int c2 = box2->GetSecondChildIndex();
Rc.MatrMultiply(boR, o2->child(c1)->Rot);
Tc = ChTrasform<>::TrasformLocalToParent(o2->child(c1)->To,boT, boR);
CollideRecurse(Rc,Tc,o1,b1,o2,c1,flag);
if ((flag == ChC_FIRST_CONTACT) && (this->GetNumPairs() > 0)) return;
Rc.MatrMultiply(boR, o2->child(c2)->Rot);
Tc = ChTrasform<>::TrasformLocalToParent(o2->child(c2)->To,boT, boR);
CollideRecurse(Rc,Tc,o1,b1,o2,c2,flag);
}
}
