コード例 #1
0
ファイル: ChCOBBcollider.cpp プロジェクト: kelaogui/chrono
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;
}
コード例 #2
0
ファイル: ChCOBBcollider.cpp プロジェクト: kelaogui/chrono
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);
    }
}