示例#1
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; 
}
示例#2
0
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;
}
示例#3
0
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;
}
示例#4
0
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);
  }
}
示例#5
0
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);
  }
}
示例#6
0
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);
    }
}