예제 #1
0
// -----------------------------------------------------------------------------
// Calculate kinematics quantities (slip angle, longitudinal slip, camber angle,
// and toe-in angle using the current state of the associated wheel body.
// -----------------------------------------------------------------------------
void ChTire::CalculateKinematics(double time, const WheelState& state, const ChTerrain& terrain) {
    // Wheel normal (expressed in global frame)
    ChVector<> wheel_normal = state.rot.GetYaxis();

    // Terrain normal at wheel location (expressed in global frame)
    ChVector<> Z_dir = terrain.GetNormal(state.pos.x(), state.pos.y());

    // Longitudinal (heading) and lateral directions, in the terrain plane
    ChVector<> X_dir = Vcross(wheel_normal, Z_dir);
    X_dir.Normalize();
    ChVector<> Y_dir = Vcross(Z_dir, X_dir);

    // Tire reference coordinate system
    ChMatrix33<> rot;
    rot.Set_A_axis(X_dir, Y_dir, Z_dir);
    ChCoordsys<> tire_csys(state.pos, rot.Get_A_quaternion());

    // Express wheel linear velocity in tire frame
    ChVector<> V = tire_csys.TransformDirectionParentToLocal(state.lin_vel);
    // Express wheel normal in tire frame
    ChVector<> n = tire_csys.TransformDirectionParentToLocal(wheel_normal);

    // Slip angle
    double abs_Vx = std::abs(V.x());
    double zero_Vx = 1e-4;
    m_slip_angle = (abs_Vx > zero_Vx) ? std::atan(V.y() / abs_Vx) : 0;

    // Longitudinal slip
    m_longitudinal_slip = (abs_Vx > zero_Vx) ? -(V.x() - state.omega * GetRadius()) / abs_Vx : 0;

    // Camber angle
    m_camber_angle = std::atan2(n.z(), n.y());
}
예제 #2
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;
}
예제 #3
0
// -----------------------------------------------------------------------------
// Utility function for characterizing the geometric contact between a disc with
// specified center location, normal direction, and radius and the terrain,
// assumed to be specified as a height field (over the x-y domain).
// This function returns false if no contact occurs. Otherwise, it sets the
// contact points on the disc (ptD) and on the terrain (ptT), the normal contact
// direction, and the resulting penetration depth (a positive value).
// -----------------------------------------------------------------------------
bool ChTire::disc_terrain_contact(const ChTerrain& terrain,
                                  const ChVector<>& disc_center,
                                  const ChVector<>& disc_normal,
                                  double disc_radius,
                                  ChCoordsys<>& contact,
                                  double& depth) {
    // Find terrain height below disc center. There is no contact if the disc
    // center is below the terrain or farther away by more than its radius.
    double hc = terrain.GetHeight(disc_center.x(), disc_center.y());
    if (disc_center.z() <= hc || disc_center.z() >= hc + disc_radius)
        return false;

    // Find the lowest point on the disc. There is no contact if the disc is
    // (almost) horizontal.
    ChVector<> dir1 = Vcross(disc_normal, ChVector<>(0, 0, 1));
    double sinTilt2 = dir1.Length2();

    if (sinTilt2 < 1e-3)
        return false;

    // Contact point (lowest point on disc).
    ChVector<> ptD = disc_center + disc_radius * Vcross(disc_normal, dir1 / sqrt(sinTilt2));

    // Find terrain height at lowest point. No contact if lowest point is above
    // the terrain.
    double hp = terrain.GetHeight(ptD.x(), ptD.y());

    if (ptD.z() > hp)
        return false;

    // Approximate the terrain with a plane. Define the projection of the lowest
    // point onto this plane as the contact point on the terrain.
    ChVector<> normal = terrain.GetNormal(ptD.x(), ptD.y());
    ChVector<> longitudinal = Vcross(disc_normal, normal);
    longitudinal.Normalize();
    ChVector<> lateral = Vcross(normal, longitudinal);
    ChMatrix33<> rot;
    rot.Set_A_axis(longitudinal, lateral, normal);

    contact.pos = ptD;
    contact.rot = rot.Get_A_quaternion();

    depth = Vdot(ChVector<>(0, 0, hp - ptD.z()), normal);
    assert(depth > 0);

    return true;
}
예제 #4
0
ChVector<double> Quat_to_Angle(AngleSet angset, const ChQuaternion<double>& mquat) {
    ChVector<double> res;
    ChMatrix33<> Acoord;

    Acoord.Set_A_quaternion(mquat);

    switch (angset) {
        case AngleSet::EULERO:
            res = Acoord.Get_A_Eulero();
            break;
        case AngleSet::CARDANO:
            res = Acoord.Get_A_Cardano();
            break;
        case AngleSet::HPB:
            res = Acoord.Get_A_Hpb();
            break;
        case AngleSet::RXYZ:
            res = Acoord.Get_A_Rxyz();
            break;
        case AngleSet::RODRIGUEZ:
            res = Acoord.Get_A_Rodriguez();
            break;
        default:
            break;
    }
    return res;
}
예제 #5
0
bool CHAABB::AABB_Overlap(ChMatrix33<>& B, Vector T, CHAABB *b1, CHAABB *b2)
{
	  ChMatrix33<> Bf;
	  const double reps = (double)1e-6;

	  // Bf = fabs(B)
	  Bf(0,0) = myfabs(B.Get33Element(0,0));  Bf(0,0) += reps;
	  Bf(0,1) = myfabs(B.Get33Element(0,1));  Bf(0,1) += reps;
	  Bf(0,2) = myfabs(B.Get33Element(0,2));  Bf(0,2) += reps;
	  Bf(1,0) = myfabs(B.Get33Element(1,0));  Bf(1,0) += reps;
	  Bf(1,1) = myfabs(B.Get33Element(1,1));  Bf(1,1) += reps;
	  Bf(1,2) = myfabs(B.Get33Element(1,2));  Bf(1,2) += reps;
	  Bf(2,0) = myfabs(B.Get33Element(2,0));  Bf(2,0) += reps;
	  Bf(2,1) = myfabs(B.Get33Element(2,1));  Bf(2,1) += reps;
	  Bf(2,2) = myfabs(B.Get33Element(2,2));  Bf(2,2) += reps;

	  return AABB_Overlap(B, Bf, T, b1, b2);
}
ChNarrowPhaseCollider::eCollSuccess ChNarrowPhaseCollider::ComputeCollisions(ChMatrix33<>& aR1,
                                                                             Vector aT1,
                                                                             ChCollisionTree* o1,
                                                                             ChMatrix33<>& aR2,
                                                                             Vector aT2,
                                                                             ChCollisionTree* o2,
                                                                             eCollMode flag) {
    // collision models must be here
    if (!o1 || !o2)
        return ChC_RESULT_GENERICERROR;

    // make sure that the models are built
    if (o1->build_state != ChCollisionTree::ChC_BUILD_STATE_PROCESSED)
        return ChC_RESULT_MODELSNOTBUILT;
    if (o2->build_state != ChCollisionTree::ChC_BUILD_STATE_PROCESSED)
        return ChC_RESULT_MODELSNOTBUILT;

    // clear the stats

    this->num_bv_tests = 0;
    this->num_geo_tests = 0;

    // don't release the memory,
    // DONT reset the num_collision_pairs counter (may be multiple calls for different object couples)
    // this->ClearPairsList();

    // Precompute useful matrices

    this->R.MatrTMultiply(aR1, aR2);  //  MTxM(this->R,R1,R2);

    static Vector Ttemp;
    Ttemp = Vsub(aT2, aT1);             // VmV(Ttemp, T2, T1);
    this->T = aR1.MatrT_x_Vect(Ttemp);  // MTxV(this->T, R1, Ttemp);

    this->T1 = aT1;
    this->T2 = aT2;
    this->R1.CopyFromMatrix(aR1);
    this->R2.CopyFromMatrix(aR2);

    //
    // CHILD CLASSES SHOULD IMPLEMENT THE REST....
    // .....(ex. code for collisions between AABB and AABB models, etc.
    //

    return ChC_RESULT_OK;
}
예제 #7
0
double ChCollisionUtils::PointTriangleDistance(Vector B,
                                               Vector A1,
                                               Vector A2,
                                               Vector A3,
                                               double& mu,
                                               double& mv,
                                               int& is_into,
                                               Vector& Bprojected) {
    // defaults
    is_into = 0;
    mu = mv = -1;
    double mdistance = 10e22;

    Vector Dx, Dy, Dz, T1, T1p;

    Dx = Vsub(A2, A1);
    Dz = Vsub(A3, A1);
    Dy = Vcross(Dz, Dx);

    double dylen = Vlength(Dy);

    if (fabs(dylen) < EPS_TRIDEGEN)  // degenerate triangle
        return mdistance;

    Dy = Vmul(Dy, 1.0 / dylen);

    ChMatrix33<> mA;
    ChMatrix33<> mAi;
    mA.Set_A_axis(Dx, Dy, Dz);

    // invert triangle coordinate matrix -if singular matrix, was degenerate triangle-.
    if (fabs(mA.FastInvert(mAi)) < 0.000001)
        return mdistance;

    T1 = mAi.Matr_x_Vect(Vsub(B, A1));
    T1p = T1;
    T1p.y() = 0;
    mu = T1.x();
    mv = T1.z();
    mdistance = -T1.y();
    if (mu >= 0 && mv >= 0 && mv <= 1.0 - mu) {
        is_into = 1;
        Bprojected = Vadd(A1, mA.Matr_x_Vect(T1p));
    }

    return mdistance;
}
예제 #8
0
void test_3()
{
	GetLog() << "\n-------------------------------------------------\n";
	GetLog() << "TEST: generic system with stiffness blocks \n\n";

	// Important: create a 'system descriptor' object that 
	// contains variables and constraints:

	ChLcpSystemDescriptor mdescriptor;

	// Now let's add variables, constraints and stiffness, as sparse data:

	mdescriptor.BeginInsertion();  // ----- system description 

		// Create C++ objects representing 'variables', set their M blocks
		// (the masses) and set their known terms 'fb'

	ChMatrix33<> minertia;
	minertia.FillDiag(6);

	ChLcpVariablesBodyOwnMass mvarA;
	mvarA.SetBodyMass(5);
	mvarA.SetBodyInertia(&minertia);
	mvarA.Get_fb().FillRandom(-3,5);

	ChLcpVariablesBodyOwnMass mvarB;
	mvarB.SetBodyMass(4);
	mvarB.SetBodyInertia(&minertia);
	mvarB.Get_fb().FillRandom(1,3);

	ChLcpVariablesBodyOwnMass mvarC;
	mvarC.SetBodyMass(5.5);
	mvarC.SetBodyInertia(&minertia);
	mvarC.Get_fb().FillRandom(-8,3);

	mdescriptor.InsertVariables(&mvarA);
	mdescriptor.InsertVariables(&mvarB);
	mdescriptor.InsertVariables(&mvarC);

		// Create two C++ objects representing 'constraints' between variables
		// and set the jacobian to random values; 
		// Also set cfm term (E diagonal = -cfm)

	ChLcpConstraintTwoBodies mca(&mvarA, &mvarB);
	mca.Set_b_i(3);
	mca.Get_Cq_a()->FillRandom(-1,1);
	mca.Get_Cq_b()->FillRandom(-1,1);
	mca.Set_cfm_i(0.2);

	ChLcpConstraintTwoBodies mcb(&mvarA, &mvarB);
	mcb.Set_b_i(5);
	mcb.Get_Cq_a()->FillRandom(-1,1);
	mcb.Get_Cq_b()->FillRandom(-1,1);
	mcb.Set_cfm_i(0.1);

	mdescriptor.InsertConstraint(&mca);
	mdescriptor.InsertConstraint(&mcb);


		// Create two C++ objects representing 'stiffness' between variables:

	ChLcpKstiffnessGeneric mKa;
	// set the affected variables (so this K is a 12x12 matrix, relative to 4 6x6 blocks)
	std::vector<ChLcpVariables*> mvarsa;
	mvarsa.push_back(&mvarA);
	mvarsa.push_back(&mvarB);
	mKa.SetVariables(mvarsa); 

	// just fill K with random values (but symmetric, by making a product of matr*matrtransposed)
	ChMatrixDynamic<> mtempA = *mKa.Get_K(); // easy init to same size of K
	mtempA.FillRandom(-0.3,0.3);
	ChMatrixDynamic<> mtempB;
	mtempB.CopyFromMatrixT(mtempA);
	*mKa.Get_K() = -mtempA*mtempB;

	mdescriptor.InsertKstiffness(&mKa);
	

	ChLcpKstiffnessGeneric mKb;
	// set the affected variables (so this K is a 12x12 matrix, relative to 4 6x6 blocks)
	std::vector<ChLcpVariables*> mvarsb;
	mvarsb.push_back(&mvarB);
	mvarsb.push_back(&mvarC);
	mKb.SetVariables(mvarsb); 

	*mKb.Get_K() =  *mKa.Get_K(); 
	
	mdescriptor.InsertKstiffness(&mKb);


	mdescriptor.EndInsertion();  // ----- system description ends here

 
	// SOLVE the problem with an iterative Krylov solver.
	// In this case we use a MINRES-like solver, that features
	// very good convergence, it supports indefinite cases (ex. 
	// redundant constraints) and also supports the presence
	// of ChStiffness blocks (other solvers cannot cope with this!)

	// .. create the solver 

	ChLcpIterativePMINRES msolver_mr(80,		// max iterations
									 false,		// don't use warm start
									 1e-12);	// termination tolerance
	
	// .. set optional parameters of solver
	msolver_mr.SetDiagonalPreconditioning(true);
	msolver_mr.SetVerbose(true);

	// .. solve the system (passing variables, constraints, stiffness
	//    blocks with the ChSystemDescriptor that we populated above)

	msolver_mr.Solve(mdescriptor);


	// .. optional: get the result as a single vector (it collects all q_i and l_i 
	//    solved values stored in variables and constraints), just for check.
	chrono::ChMatrixDynamic<double> mx;
	mdescriptor.FromUnknownsToVector(mx);		// x ={q,-l}



	// CHECK. Test if, with the solved x, we really have Z*x-d=0 ...
	// to this end do the multiplication with the special function
	// SystemProduct() that is 'sparse-friendly' and does not build Z explicitly:
	
	chrono::ChMatrixDynamic<double> md;
	mdescriptor.BuildDiVector(md);				// d={f;-b}

	chrono::ChMatrixDynamic<double> mZx;
	mdescriptor.SystemProduct(mZx, &mx);		// Zx = Z*x
	
	GetLog() << "CHECK: norm of solver residual: ||Z*x-d|| -------------------\n";
	GetLog() << (mZx - md).NormInf() << "\n";

	/*
	// Alternatively, instead of using FromUnknownsToVector, to fetch
	// result, you could just loop over the variables (q values) and 
	// over the constraints (l values), as already shown in previous examples:

	for (int im = 0; im < mdescriptor.GetVariablesList().size(); im++)
		GetLog() << "   " << mdescriptor.GetVariablesList()[im]->Get_qb()(0) << "\n";

	for (int ic = 0; ic < mdescriptor.GetConstraintsList().size(); ic++)
		GetLog() << "   " << mdescriptor.GetConstraintsList()[ic]->Get_l_i() << "\n"; 
	*/

	



	

}
예제 #9
0
void ChCapsule::CovarianceMatrix(ChMatrix33<>& C) const {
    C.Reset();
    C(0, 0) = center.x() * center.x();
    C(1, 1) = center.y() * center.y();
    C(2, 2) = center.z() * center.z();
}
// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
void ChLinearDamperRWAssembly::Initialize(std::shared_ptr<ChBodyAuxRef> chassis, const ChVector<>& location) {
    // Express the suspension reference frame in the absolute coordinate system.
    ChFrame<> susp_to_abs(location);
    susp_to_abs.ConcatenatePreTransformation(chassis->GetFrame_REF_to_abs());

    // Transform all points and directions to absolute frame.
    std::vector<ChVector<> > points(NUM_POINTS);

    for (int i = 0; i < NUM_POINTS; i++) {
        ChVector<> rel_pos = GetLocation(static_cast<PointId>(i));
        points[i] = susp_to_abs.TransformPointLocalToParent(rel_pos);
    }

    // Create the trailing arm body. The reference frame of the arm body has its
    // x-axis aligned with the line between the arm-chassis connection point and
    // the arm-wheel connection point.
    ChVector<> y_dir = susp_to_abs.GetA().Get_A_Yaxis();
    ChVector<> u = susp_to_abs.GetPos() - points[ARM_CHASSIS];
    u.Normalize();
    ChVector<> w = Vcross(u, y_dir);
    w.Normalize();
    ChVector<> v = Vcross(w, u);
    ChMatrix33<> rot;
    rot.Set_A_axis(u, v, w);

    m_arm = std::shared_ptr<ChBody>(chassis->GetSystem()->NewBody());
    m_arm->SetNameString(m_name + "_arm");
    m_arm->SetPos(points[ARM]);
    m_arm->SetRot(rot);
    m_arm->SetMass(GetArmMass());
    m_arm->SetInertiaXX(GetArmInertia());
    chassis->GetSystem()->AddBody(m_arm);

    // Cache points and directions for arm visualization (expressed in the arm frame)
    m_pO = m_arm->TransformPointParentToLocal(susp_to_abs.GetPos());
    m_pA = m_arm->TransformPointParentToLocal(points[ARM]);
    m_pAW = m_arm->TransformPointParentToLocal(points[ARM_WHEEL]);
    m_pAC = m_arm->TransformPointParentToLocal(points[ARM_CHASSIS]);
    m_pAS = m_arm->TransformPointParentToLocal(points[SHOCK_A]);
    m_dY = m_arm->TransformDirectionParentToLocal(y_dir);

    // Create and initialize the revolute joint between arm and chassis.
    // The axis of rotation is the y axis of the suspension reference frame.
    m_revolute = std::make_shared<ChLinkLockRevolute>();
    m_revolute->SetNameString(m_name + "_revolute");
    m_revolute->Initialize(chassis, m_arm,
                           ChCoordsys<>(points[ARM_CHASSIS], susp_to_abs.GetRot() * Q_from_AngX(CH_C_PI_2)));
    chassis->GetSystem()->AddLink(m_revolute);

    // Create and initialize the rotational spring torque element.
    m_spring = std::make_shared<ChLinkRotSpringCB>();
    m_spring->SetNameString(m_name + "_spring");
    m_spring->Initialize(chassis, m_arm, ChCoordsys<>(points[ARM_CHASSIS], susp_to_abs.GetRot() * Q_from_AngX(CH_C_PI_2)));
    m_spring->RegisterTorqueFunctor(GetSpringTorqueFunctor());
    chassis->GetSystem()->AddLink(m_spring);

    // Create and initialize the translational shock force element.
    if (m_has_shock) {
        m_shock = std::make_shared<ChLinkSpringCB>();
        m_shock->SetNameString(m_name + "_shock");
        m_shock->Initialize(chassis, m_arm, false, points[SHOCK_C], points[SHOCK_A]);
        m_shock->RegisterForceFunctor(GetShockForceFunctor());
        chassis->GetSystem()->AddLink(m_shock);
    }

    // Invoke the base class implementation. This initializes the associated road wheel.
    // Note: we must call this here, after the m_arm body is created.
    ChRoadWheelAssembly::Initialize(chassis, location);
}
예제 #11
0
void ChModelBullet::_injectShape(ChVector<>* pos, ChMatrix33<>* rot, btCollisionShape* mshape)
{
	ChVector<>   defpos = VNULL;
	ChMatrix33<> defrot;
	defrot.Set33Identity();

	if (!pos)
		pos = &defpos;
	if (!rot)
		rot = &defrot;

	bool centered = false;
	if ((*pos==defpos)&&(*rot==defrot))
		centered = true;

	// start_vector = ||    -- description is still empty
	if (shapes.size()==0)
	{
		if (centered)
		{
			 shapes.push_back(ChSmartPtr<btCollisionShape>(mshape)); 
			bt_collision_object->setCollisionShape(mshape);
			// end_vector=  | centered shape | 
			return;
		}
		else
		{
			btCompoundShape* mcompound = new btCompoundShape(true);
			 shapes.push_back(ChSmartPtr<btCollisionShape>(mcompound)); 
			 shapes.push_back(ChSmartPtr<btCollisionShape>(mshape)); 
			bt_collision_object->setCollisionShape(mcompound);
			btTransform mtrasform;
			ChPosMatrToBullet(pos,rot, mtrasform);
			mcompound->addChildShape(mtrasform, mshape);
			// vector=  | compound | not centered shape |
			return;
		}
	}
	// start_vector = | centered shape |    ----just a single centered shape was added
	if (shapes.size()==1)
	{
		btTransform mtrasform;
		 shapes.push_back(shapes[0]);
		 shapes.push_back(ChSmartPtr<btCollisionShape>(mshape));
		btCompoundShape* mcompound = new btCompoundShape(true);
		 shapes[0] = ChSmartPtr<btCollisionShape>(mcompound); 
		bt_collision_object->setCollisionShape(mcompound);
		ChPosMatrToBullet(&defpos, &defrot, mtrasform);
		mcompound->addChildShape(mtrasform, shapes[1].get_ptr()); 
		ChPosMatrToBullet(pos,rot, mtrasform);
		mcompound->addChildShape(mtrasform, shapes[2].get_ptr()); 
		// vector=  | compound | old centered shape | new shape | ...
		return;
	}
	// vector=  | compound | old | old.. |   ----already working with compounds..
	if (shapes.size()>1)
	{
		btTransform mtrasform;
		 shapes.push_back(ChSmartPtr<btCollisionShape>(mshape)); 
		ChPosMatrToBullet(pos,rot, mtrasform);
		btCollisionShape* mcom = shapes[0].get_ptr(); 
		((btCompoundShape*)mcom)->addChildShape(mtrasform, mshape); 
		// vector=  | compound | old | old.. | new shape | ...
		return;
	}
}
예제 #12
0
// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
void ChPitmanArm::Initialize(std::shared_ptr<ChBodyAuxRef> chassis,
                             const ChVector<>& location,
                             const ChQuaternion<>& rotation) {
    m_position = ChCoordsys<>(location, rotation);

    // Chassis orientation (expressed in absolute frame)
    // Recall that the suspension reference frame is aligned with the chassis.
    ChQuaternion<> chassisRot = chassis->GetFrame_REF_to_abs().GetRot();

    // Express the steering reference frame in the absolute coordinate system.
    ChFrame<> steering_to_abs(location, rotation);
    steering_to_abs.ConcatenatePreTransformation(chassis->GetFrame_REF_to_abs());

    // Transform all points and directions to absolute frame.
    std::vector<ChVector<>> points(NUM_POINTS);
    std::vector<ChVector<>> dirs(NUM_DIRS);

    for (int i = 0; i < NUM_POINTS; i++) {
        ChVector<> rel_pos = getLocation(static_cast<PointId>(i));
        points[i] = steering_to_abs.TransformPointLocalToParent(rel_pos);
    }

    for (int i = 0; i < NUM_DIRS; i++) {
        ChVector<> rel_dir = getDirection(static_cast<DirectionId>(i));
        dirs[i] = steering_to_abs.TransformDirectionLocalToParent(rel_dir);
    }

    // Unit vectors for orientation matrices.
    ChVector<> u;
    ChVector<> v;
    ChVector<> w;
    ChMatrix33<> rot;

    // Create and initialize the steering link body
    m_link = std::shared_ptr<ChBody>(chassis->GetSystem()->NewBody());
    m_link->SetNameString(m_name + "_link");
    m_link->SetPos(points[STEERINGLINK]);
    m_link->SetRot(steering_to_abs.GetRot());
    m_link->SetMass(getSteeringLinkMass());
    if (m_vehicle_frame_inertia) {
        ChMatrix33<> inertia = TransformInertiaMatrix(getSteeringLinkInertiaMoments(), getSteeringLinkInertiaProducts(),
                                                      chassisRot, steering_to_abs.GetRot());
        m_link->SetInertia(inertia);
    } else {
        m_link->SetInertiaXX(getSteeringLinkInertiaMoments());
        m_link->SetInertiaXY(getSteeringLinkInertiaProducts());
    }
    chassis->GetSystem()->AddBody(m_link);

    m_pP = m_link->TransformPointParentToLocal(points[UNIV]);
    m_pI = m_link->TransformPointParentToLocal(points[REVSPH_S]);
    m_pTP = m_link->TransformPointParentToLocal(points[TIEROD_PA]);
    m_pTI = m_link->TransformPointParentToLocal(points[TIEROD_IA]);

    // Create and initialize the Pitman arm body
    m_arm = std::shared_ptr<ChBody>(chassis->GetSystem()->NewBody());
    m_arm->SetNameString(m_name + "_arm");
    m_arm->SetPos(points[PITMANARM]);
    m_arm->SetRot(steering_to_abs.GetRot());
    m_arm->SetMass(getPitmanArmMass());
    if (m_vehicle_frame_inertia) {
        ChMatrix33<> inertia = TransformInertiaMatrix(getPitmanArmInertiaMoments(), getPitmanArmInertiaProducts(),
                                                      chassisRot, steering_to_abs.GetRot());
        m_arm->SetInertia(inertia);
    } else {
        m_arm->SetInertiaXX(getPitmanArmInertiaMoments());
        m_arm->SetInertiaXY(getPitmanArmInertiaProducts());
    }
    chassis->GetSystem()->AddBody(m_arm);

    // Cache points for arm visualization (expressed in the arm frame)
    m_pC = m_arm->TransformPointParentToLocal(points[REV]);
    m_pL = m_arm->TransformPointParentToLocal(points[UNIV]);

    // Create and initialize the revolute joint between chassis and Pitman arm.
    // Note that this is modeled as a ChLinkEngine to allow driving it with
    // imposed rotation (steering input).
    // The z direction of the joint orientation matrix is dirs[REV_AXIS], assumed
    // to be a unit vector.
    u = points[PITMANARM] - points[REV];
    v = Vcross(dirs[REV_AXIS], u);
    v.Normalize();
    u = Vcross(v, dirs[REV_AXIS]);
    rot.Set_A_axis(u, v, dirs[REV_AXIS]);

    m_revolute = std::make_shared<ChLinkMotorRotationAngle>();
    m_revolute->SetNameString(m_name + "_revolute");
    m_revolute->Initialize(chassis, m_arm, ChFrame<>(points[REV], rot.Get_A_quaternion()));
    auto motor_fun = std::make_shared<ChFunction_Setpoint>();
    m_revolute->SetAngleFunction(motor_fun);
    chassis->GetSystem()->AddLink(m_revolute);

    // Create and initialize the universal joint between the Pitman arm and steering link.
    // The x and y directions of the joint orientation matrix are given by
    // dirs[UNIV_AXIS_ARM] and dirs[UNIV_AXIS_LINK], assumed to be unit vectors
    // and orthogonal.
    w = Vcross(dirs[UNIV_AXIS_ARM], dirs[UNIV_AXIS_LINK]);
    rot.Set_A_axis(dirs[UNIV_AXIS_ARM], dirs[UNIV_AXIS_LINK], w);

    m_universal = std::make_shared<ChLinkUniversal>();
    m_universal->SetNameString(m_name + "_universal");
    m_universal->Initialize(m_arm, m_link, ChFrame<>(points[UNIV], rot.Get_A_quaternion()));
    chassis->GetSystem()->AddLink(m_universal);

    // Create and initialize the revolute-spherical joint (massless idler arm).
    // The length of the idler arm is the distance between the two hardpoints.
    // The z direction of the revolute joint orientation matrix is
    // dirs[REVSPH_AXIS], assumed to be a unit vector.
    double distance = (points[REVSPH_S] - points[REVSPH_R]).Length();

    u = points[REVSPH_S] - points[REVSPH_R];
    v = Vcross(dirs[REVSPH_AXIS], u);
    v.Normalize();
    u = Vcross(v, dirs[REVSPH_AXIS]);
    rot.Set_A_axis(u, v, dirs[REVSPH_AXIS]);

    m_revsph = std::make_shared<ChLinkRevoluteSpherical>();
    m_revsph->SetNameString(m_name + "_revsph");
    m_revsph->Initialize(chassis, m_link, ChCoordsys<>(points[REVSPH_R], rot.Get_A_quaternion()), distance);
    chassis->GetSystem()->AddLink(m_revsph);
}
예제 #13
0
void ChLinkGear::UpdateTime (double mytime)
{
    // First, inherit to parent class
    ChLinkLock::UpdateTime(mytime);

    // Move markers 1 and 2 to align them as gear teeth

    ChMatrix33<> ma1;
    ChMatrix33<> ma2;
    ChMatrix33<> mrotma;
    ChMatrix33<> marot_beta;
    Vector mx;
    Vector my;
    Vector mz;
	Vector mr;
    Vector mmark1;
    Vector mmark2;
    Vector lastX;
    Vector vrota;
    Coordsys newmarkpos;

	ChFrame<double> abs_shaft1;
	ChFrame<double> abs_shaft2;

	((ChFrame<double>*)Body1)->TrasformLocalToParent(local_shaft1, abs_shaft1);
	((ChFrame<double>*)Body2)->TrasformLocalToParent(local_shaft2, abs_shaft2);

    Vector vbdist = Vsub(Get_shaft_pos1(),
                          Get_shaft_pos2());
    Vector Trad1 = Vnorm(Vcross(Get_shaft_dir1(), Vnorm(Vcross(Get_shaft_dir1(),vbdist))));
    Vector Trad2 = Vnorm(Vcross(Vnorm(Vcross(Get_shaft_dir2(),vbdist)), Get_shaft_dir2()));

	double dist = Vlenght(vbdist);
    

        // compute actual rotation of the two wheels (relative to truss).
    Vector md1 = abs_shaft1.GetA()->MatrT_x_Vect(-vbdist);
    md1.z = 0;  md1 = Vnorm (md1);
    Vector md2 = abs_shaft2.GetA()->MatrT_x_Vect(-vbdist);
    md2.z = 0;  md2 = Vnorm (md2);

	double periodic_a1 = ChAtan2(md1.x, md1.y);
	double periodic_a2 = ChAtan2(md2.x, md2.y);
	double old_a1 = a1; 
	double old_a2 = a2;
	double turns_a1 = floor (old_a1 / CH_C_2PI);
	double turns_a2 = floor (old_a2 / CH_C_2PI);
	double a1U = turns_a1 * CH_C_2PI + periodic_a1 + CH_C_2PI;
	double a1M = turns_a1 * CH_C_2PI + periodic_a1;
	double a1L = turns_a1 * CH_C_2PI + periodic_a1 - CH_C_2PI;
	a1 = a1M;
	if (fabs(a1U - old_a1) < fabs(a1M - old_a1))
		a1 = a1U;
	if (fabs(a1L - a1) < fabs(a1M - a1))
		a1 = a1L;
	double a2U = turns_a2 * CH_C_2PI + periodic_a2 + CH_C_2PI;
	double a2M = turns_a2 * CH_C_2PI + periodic_a2;
	double a2L = turns_a2 * CH_C_2PI + periodic_a2 - CH_C_2PI;
	a2 = a2M;
	if (fabs(a2U - old_a2) < fabs(a2M - old_a2))
		a2 = a2U;
	if (fabs(a2L - a2) < fabs(a2M - a2))
		a2 = a2L;


        // compute new markers coordsystem alignment
    my = Vnorm (vbdist);
    mz = Get_shaft_dir1();
    mx = Vnorm(Vcross (my, mz));
	mr = Vnorm(Vcross (mz, mx));
    mz = Vnorm(Vcross (mx, my));
	ChVector<> mz2, mx2, mr2, my2;
	my2 = my;
	mz2 = Get_shaft_dir2();
	mx2 = Vnorm(Vcross (my2, mz2));
	mr2 = Vnorm(Vcross (mz2, mx2));

    ma1.Set_A_axis(mx,my,mz);

        // rotate csys because of beta
    vrota.x = 0.0;  vrota.y = this->beta;  vrota.z = 0.0;
    mrotma.Set_A_Rxyz(vrota);
    marot_beta.MatrMultiply(ma1, mrotma);
        // rotate csys because of alpha
    vrota.x = 0.0;  vrota.y = 0.0;  vrota.z = this->alpha;
    if (react_force.x < 0)  vrota.z =  this->alpha;
    else                    vrota.z = -this->alpha;
    mrotma.Set_A_Rxyz(vrota);
    ma1.MatrMultiply(marot_beta, mrotma);

    ma2.CopyFromMatrix(ma1);

		// is a bevel gear?
	double be = acos(Vdot(Get_shaft_dir1(), Get_shaft_dir2()));
	bool is_bevel= true;
	if (fabs( Vdot(Get_shaft_dir1(), Get_shaft_dir2()) )>0.96)
		is_bevel = false;

        // compute wheel radii
        // so that:
        //            w2 = - tau * w1
	if (!is_bevel)
	{
		double pardist =  Vdot(mr, vbdist);
		double inv_tau = 1.0/tau;
		if  (!this->epicyclic)
		{
			r2 = pardist - pardist / (inv_tau + 1.0);
		}
		else
		{
			r2 = pardist - (tau * pardist)/(tau-1.0);
		}
		r1 = r2*tau;	}
	else
	{
		double gamma2;
		if  (!this->epicyclic)
		{
			gamma2 = be/(tau + 1.0);
		}
		else
		{
			gamma2 = be/(-tau + 1.0);
		}
	   double al = CH_C_PI - acos (Vdot(Get_shaft_dir2(), my));
		double te = CH_C_PI - al - be;
		double fd = sin(te) * (dist/sin(be));
		r2 = fd * tan(gamma2);
		r1 = r2*tau;
	}

        // compute markers positions, supposing they
        // stay on the ideal wheel contact point
	mmark1 = Vadd(Get_shaft_pos2(), Vmul(mr2, r2));
    mmark2 = mmark1;
    contact_pt = mmark1;

        // correct marker 1 position if phasing is not correct
    if (this->checkphase)
    {
		double realtau = tau; 
		if (this->epicyclic) 
			realtau = -tau;
        double m_delta;
        m_delta = - (a2/realtau) - a1 - phase;

        if (m_delta> CH_C_PI) m_delta -= (CH_C_2PI);		 // range -180..+180 is better than 0...360
        if (m_delta> (CH_C_PI/4.0)) m_delta = (CH_C_PI/4.0); // phase correction only in +/- 45°
        if (m_delta<-(CH_C_PI/4.0)) m_delta =-(CH_C_PI/4.0);

        vrota.x = vrota.y = 0.0;  vrota.z = - m_delta;
        mrotma.Set_A_Rxyz(vrota);   // rotate about Z of shaft to correct
        mmark1 = abs_shaft1.GetA()->MatrT_x_Vect(Vsub(mmark1,  Get_shaft_pos1() ));
        mmark1 = mrotma.Matr_x_Vect(mmark1);
        mmark1 = Vadd (abs_shaft1.GetA()->Matr_x_Vect(mmark1), Get_shaft_pos1() );
    }
		// Move Shaft 1 along its direction if not aligned to wheel
	double offset =  Vdot (this->Get_shaft_dir1(), (contact_pt - this->Get_shaft_pos1()) );
	ChVector<> moff = this->Get_shaft_dir1() * offset;
	if (fabs (offset) > 0.0001)
		this->local_shaft1.SetPos( local_shaft1.GetPos() + Body1->Dir_World2Body(&moff) );
		

            // ! Require that the BDF routine of marker won't handle speed and acc.calculus of the moved marker 2!
    marker2->SetMotionType(ChMarker::M_MOTION_EXTERNAL);
    marker1->SetMotionType(ChMarker::M_MOTION_EXTERNAL);

        // move marker1 in proper positions
    newmarkpos.pos = mmark1;
    newmarkpos.rot = ma1.Get_A_quaternion();
    marker1->Impose_Abs_Coord(newmarkpos);        //move marker1 into teeth position
        // move marker2 in proper positions
    newmarkpos.pos = mmark2;
    newmarkpos.rot = ma2.Get_A_quaternion();
    marker2->Impose_Abs_Coord(newmarkpos);        //move marker2 into teeth position


        // imposed relative positions/speeds
    deltaC.pos = VNULL;
    deltaC_dt.pos = VNULL;
    deltaC_dtdt.pos = VNULL;

    deltaC.rot = QUNIT;             // no relative rotations imposed!
    deltaC_dt.rot = QNULL;
    deltaC_dtdt.rot = QNULL;

}
예제 #14
0
void ChLinkPulley::UpdateTime (double mytime)
{
    // First, inherit to parent class
    ChLinkLock::UpdateTime(mytime);

	ChFrame<double> abs_shaft1;
	ChFrame<double> abs_shaft2;

	((ChFrame<double>*)Body1)->TrasformLocalToParent(local_shaft1, abs_shaft1);
	((ChFrame<double>*)Body2)->TrasformLocalToParent(local_shaft2, abs_shaft2);

	ChVector<> dcc_w = Vsub(Get_shaft_pos2(),
                            Get_shaft_pos1());

		// compute actual rotation of the two wheels (relative to truss).
    Vector md1 = abs_shaft1.GetA()->MatrT_x_Vect(dcc_w);
    md1.z = 0;  md1 = Vnorm (md1);
    Vector md2 = abs_shaft2.GetA()->MatrT_x_Vect(dcc_w);
    md2.z = 0;  md2 = Vnorm (md2);

	double periodic_a1 = ChAtan2(md1.x, md1.y);
	double periodic_a2 = ChAtan2(md2.x, md2.y);
	double old_a1 = a1; 
	double old_a2 = a2;
	double turns_a1 = floor (old_a1 / CH_C_2PI);
	double turns_a2 = floor (old_a2 / CH_C_2PI);
	double a1U = turns_a1 * CH_C_2PI + periodic_a1 + CH_C_2PI;
	double a1M = turns_a1 * CH_C_2PI + periodic_a1;
	double a1L = turns_a1 * CH_C_2PI + periodic_a1 - CH_C_2PI;
	a1 = a1M;
	if (fabs(a1U - old_a1) < fabs(a1M - old_a1))
		a1 = a1U;
	if (fabs(a1L - a1) < fabs(a1M - a1))
		a1 = a1L;
	double a2U = turns_a2 * CH_C_2PI + periodic_a2 + CH_C_2PI;
	double a2M = turns_a2 * CH_C_2PI + periodic_a2;
	double a2L = turns_a2 * CH_C_2PI + periodic_a2 - CH_C_2PI;
	a2 = a2M;
	if (fabs(a2U - old_a2) < fabs(a2M - old_a2))
		a2 = a2U;
	if (fabs(a2L - a2) < fabs(a2M - a2))
		a2 = a2L;

	     // correct marker positions if phasing is not correct
	double m_delta =0;
    if (this->checkphase)
    {
		double realtau = tau; 
		//if (this->epicyclic) 
		//	realtau = -tau;
        
        m_delta = a1 - phase - (a2/realtau);

        if (m_delta> CH_C_PI) m_delta -= (CH_C_2PI);		 // range -180..+180 is better than 0...360
        if (m_delta> (CH_C_PI/4.0)) m_delta = (CH_C_PI/4.0); // phase correction only in +/- 45°
        if (m_delta<-(CH_C_PI/4.0)) m_delta =-(CH_C_PI/4.0);
		//***TODO***
    }


    // Move markers 1 and 2 to align them as pulley ends

	ChVector<> d21_w = dcc_w - Get_shaft_dir1()* Vdot (Get_shaft_dir1(), dcc_w);
	ChVector<> D21_w = Vnorm(d21_w);

	this->shaft_dist = d21_w.Length();
	
	ChVector<> U1_w = Vcross(Get_shaft_dir1(), D21_w);

	double gamma1 = acos( (r1-r2) / shaft_dist);

	ChVector<> Ru_w =  D21_w*cos(gamma1) + U1_w*sin(gamma1);
	ChVector<> Rl_w =  D21_w*cos(gamma1) - U1_w*sin(gamma1);

	this->belt_up1  = Get_shaft_pos1()+ Ru_w*r1;
	this->belt_low1 = Get_shaft_pos1()+ Rl_w*r1;
	this->belt_up2  = Get_shaft_pos1()+ d21_w + Ru_w*r2;
	this->belt_low2 = Get_shaft_pos1()+ d21_w + Rl_w*r2;

		// marker alignment
	ChMatrix33<> maU;
	ChMatrix33<> maL;

	ChVector<> Dxu = Vnorm(belt_up2 - belt_up1);
	ChVector<> Dyu = Ru_w;
	ChVector<> Dzu = Vnorm (Vcross(Dxu, Dyu));
	Dyu = Vnorm (Vcross(Dzu, Dxu));
	maU.Set_A_axis(Dxu,Dyu,Dzu);

            // ! Require that the BDF routine of marker won't handle speed and acc.calculus of the moved marker 2!
    marker2->SetMotionType(ChMarker::M_MOTION_EXTERNAL);
    marker1->SetMotionType(ChMarker::M_MOTION_EXTERNAL);

	ChCoordsys<> newmarkpos;

        // move marker1 in proper positions
    newmarkpos.pos = this->belt_up1;
    newmarkpos.rot = maU.Get_A_quaternion();
    marker1->Impose_Abs_Coord(newmarkpos);        //move marker1 into teeth position
        // move marker2 in proper positions
    newmarkpos.pos = this->belt_up2;
    newmarkpos.rot = maU.Get_A_quaternion();
    marker2->Impose_Abs_Coord(newmarkpos);        //move marker2 into teeth position

	double phase_correction_up = m_delta*r1;
	double phase_correction_low = - phase_correction_up;
	double hU = Vlenght(belt_up2- belt_up1)  + phase_correction_up;
	double hL = Vlenght(belt_low2- belt_low1) + phase_correction_low;

        // imposed relative positions/speeds
    deltaC.pos = ChVector<>(-hU, 0, 0);
    deltaC_dt.pos = VNULL;
    deltaC_dtdt.pos = VNULL;

    deltaC.rot = QUNIT;             // no relative rotations imposed!
    deltaC_dt.rot = QNULL;
    deltaC_dtdt.rot = QNULL;
}
예제 #15
0
bool CHAABB::AABB_Overlap(ChMatrix33<>& B, ChMatrix33<>& Bf, Vector T, CHAABB *b1, CHAABB *b2)
{
	  register double t, s;
	  register int r;

	  Vector& a = b1->d;
	  Vector& b = b2->d;

	  // if any of these tests are one-sided, then the polyhedra are disjoint
	  r = 1;

	  // A1 x A2 = A0
	  t = myfabs(T.x);

	  r &= (t <=
		  (a.x + b.x * Bf.Get33Element(0,0) + b.y * Bf.Get33Element(0,1) + b.z * Bf.Get33Element(0,2)));
	  if (!r) return false;

	  // B1 x B2 = B0
	  s = T.x*B.Get33Element(0,0) + T.y*B.Get33Element(1,0) + T.z*B.Get33Element(2,0);
	  t = myfabs(s);

	  r &= ( t <=
		  (b.x + a.x * Bf.Get33Element(0,0) + a.y * Bf.Get33Element(1,0) + a.z * Bf.Get33Element(2,0)));
	  if (!r) return false;

	  // A2 x A0 = A1
	  t = myfabs(T.y);

	  r &= ( t <=
		  (a.y + b.x * Bf.Get33Element(1,0) + b.y * Bf.Get33Element(1,1) + b.z * Bf.Get33Element(1,2)));
	  if (!r) return false;

	  // A0 x A1 = A2
	  t = myfabs(T.z);

	  r &= ( t <=
		  (a.z + b.x * Bf.Get33Element(2,0) + b.y * Bf.Get33Element(2,1) + b.z * Bf.Get33Element(2,2)));
	  if (!r) return false;

	  // B2 x B0 = B1
	  s = T.x*B.Get33Element(0,1) + T.y*B.Get33Element(1,1) + T.z*B.Get33Element(2,1);
	  t = myfabs(s);

	  r &= ( t <=
		  (b.y + a.x * Bf.Get33Element(0,1) + a.y * Bf.Get33Element(1,1) + a.z * Bf.Get33Element(2,1)));
	  if (!r) return false;

	  // B0 x B1 = B2
	  s = T.x*B.Get33Element(0,2) + T.y*B.Get33Element(1,2) + T.z*B.Get33Element(2,2);
	  t = myfabs(s);

	  r &= ( t <=
		  (b.z + a.x * Bf.Get33Element(0,2) + a.y * Bf.Get33Element(1,2) + a.z * Bf.Get33Element(2,2)));
	  if (!r) return false;

	  // A0 x B0
	  s = T.z * B.Get33Element(1,0) - T.y * B.Get33Element(2,0);
	  t = myfabs(s);

	  r &= ( t <=
		(a.y * Bf.Get33Element(2,0) + a.z * Bf.Get33Element(1,0) +
		 b.y * Bf.Get33Element(0,2) + b.z * Bf.Get33Element(0,1)));
	  if (!r) return false;

	  // A0 x B1
	  s = T.z * B.Get33Element(1,1) - T.y * B.Get33Element(2,1);
	  t = myfabs(s);

	  r &= ( t <=
		(a.y * Bf.Get33Element(2,1) + a.z * Bf.Get33Element(1,1) +
		 b.x * Bf.Get33Element(0,2) + b.z * Bf.Get33Element(0,0)));
	  if (!r) return false;

	  // A0 x B2
	  s = T.z * B.Get33Element(1,2) - T.y * B.Get33Element(2,2);
	  t = myfabs(s);

	  r &= ( t <=
		  (a.y * Bf.Get33Element(2,2) + a.z * Bf.Get33Element(1,2) +
		   b.x * Bf.Get33Element(0,1) + b.y * Bf.Get33Element(0,0)));
	  if (!r) return false;

	  // A1 x B0
	  s = T.x * B.Get33Element(2,0) - T.z * B.Get33Element(0,0);
	  t = myfabs(s);

	  r &= ( t <=
		  (a.x * Bf.Get33Element(2,0) + a.z * Bf.Get33Element(0,0) +
		   b.y * Bf.Get33Element(1,2) + b.z * Bf.Get33Element(1,1)));
	  if (!r) return false;

	  // A1 x B1
	  s = T.x * B.Get33Element(2,1) - T.z * B.Get33Element(0,1);
	  t = myfabs(s);

	  r &= ( t <=
		  (a.x * Bf.Get33Element(2,1) + a.z * Bf.Get33Element(0,1) +
		   b.x * Bf.Get33Element(1,2) + b.z * Bf.Get33Element(1,0)));
	  if (!r) return false;

	  // A1 x B2
	  s = T.x * B.Get33Element(2,2) - T.z * B.Get33Element(0,2);
	  t = myfabs(s);

	  r &= (t <=
		  (a.x * Bf.Get33Element(2,2) + a.z * Bf.Get33Element(0,2) +
		   b.x * Bf.Get33Element(1,1) + b.y * Bf.Get33Element(1,0)));
	  if (!r) return false;

	  // A2 x B0
	  s = T.y * B.Get33Element(0,0) - T.x * B.Get33Element(1,0);
	  t = myfabs(s);

	  r &= (t <=
		  (a.x * Bf.Get33Element(1,0) + a.y * Bf.Get33Element(0,0) +
		   b.y * Bf.Get33Element(2,2) + b.z * Bf.Get33Element(2,1)));
	  if (!r) return false;

	  // A2 x B1
	  s = T.y * B.Get33Element(0,1) - T.x * B.Get33Element(1,1);
	  t = myfabs(s);

	  r &= ( t <=
		  (a.x * Bf.Get33Element(1,1) + a.y * Bf.Get33Element(0,1) +
		   b.x * Bf.Get33Element(2,2) + b.z * Bf.Get33Element(2,0)));
	  if (!r) return false;

	  // A2 x B2
	  s = T.y * B.Get33Element(0,2) - T.x * B.Get33Element(1,2);
	  t = myfabs(s);

	  r &= ( t <=
		  (a.x * Bf.Get33Element(1,2) + a.y * Bf.Get33Element(0,2) +
		   b.x * Bf.Get33Element(2,1) + b.y * Bf.Get33Element(2,0)));
	  if (!r) return false;

	  return true;  // no separation: BV collide
}
예제 #16
0
void HMMWV_ReissnerTire::CreateMesh(const ChFrameMoving<>& wheel_frame, VehicleSide side) {
    // Create piece-wise cubic spline approximation of the tire profile.
    //   x - radial direction
    //   y - transversal direction
    ChCubicSpline splineX(m_profile_t, m_profile_x);
    ChCubicSpline splineY(m_profile_t, m_profile_y);

    // Create the mesh nodes.
    // The nodes are first created in the wheel local frame, assuming Y as the tire axis,
    // and are then transformed to the global frame.
    for (int i = 0; i < m_div_circumference; i++) {
        double phi = (CH_C_2PI * i) / m_div_circumference;
        ChVector<> nrm(-std::sin(phi), 0, std::cos(phi));

        for (int j = 0; j <= m_div_width; j++) {
            double t_prf = double(j) / m_div_width;
            double x_prf, xp_prf, xpp_prf;
            double y_prf, yp_prf, ypp_prf;
            splineX.Evaluate(t_prf, x_prf, xp_prf, xpp_prf);
            splineY.Evaluate(t_prf, y_prf, yp_prf, ypp_prf);

            // Node position with respect to rim center
            double x = (m_rim_radius + x_prf) * std::cos(phi);
            double y = y_prf;
            double z = (m_rim_radius + x_prf) * std::sin(phi);
            // Node position in global frame (actual coordinate values)
            ChVector<> loc = wheel_frame.TransformPointLocalToParent(ChVector<>(x, y, z));

            // Node direction
            ChVector<> tan_prf(std::cos(phi) * xp_prf, yp_prf, std::sin(phi) * xp_prf);
            ChVector<> nrm_prf = Vcross(tan_prf, nrm).GetNormalized();
            ChVector<> dir = wheel_frame.TransformDirectionLocalToParent(nrm_prf);
            ChMatrix33<> mrot; mrot.Set_A_Xdir(tan_prf,nrm_prf);
            auto node = std::make_shared<ChNodeFEAxyzrot>(ChFrame<>(loc, mrot));

            // Node velocity
            ChVector<> vel = wheel_frame.PointSpeedLocalToParent(ChVector<>(x, y, z));
            node->SetPos_dt(vel);
            node->SetMass(0);
            m_mesh->AddNode(node);
        }
    }

    // Create the Reissner shell elements
    for (int i = 0; i < m_div_circumference; i++) {
        for (int j = 0; j < m_div_width; j++) {
            // Adjacent nodes
            int inode0, inode1, inode2, inode3;
            inode1 = j + i * (m_div_width + 1);
            inode2 = j + 1 + i * (m_div_width + 1);
            if (i == m_div_circumference - 1) {
                inode0 = j;
                inode3 = j + 1;
            } else {
                inode0 = j + (i + 1) * (m_div_width + 1);
                inode3 = j + 1 + (i + 1) * (m_div_width + 1);
            }

            auto node0 = std::dynamic_pointer_cast<ChNodeFEAxyzrot>(m_mesh->GetNode(inode0));
            auto node1 = std::dynamic_pointer_cast<ChNodeFEAxyzrot>(m_mesh->GetNode(inode1));
            auto node2 = std::dynamic_pointer_cast<ChNodeFEAxyzrot>(m_mesh->GetNode(inode2));
            auto node3 = std::dynamic_pointer_cast<ChNodeFEAxyzrot>(m_mesh->GetNode(inode3));

            // Create the element and set its nodes.
            auto element = std::make_shared<ChElementShellReissner4>();
            element->SetNodes(node0, node1, node2, node3);

            // Element dimensions
            double len_circumference =
                0.5 * ((node1->GetPos() - node0->GetPos()).Length() + (node3->GetPos() - node2->GetPos()).Length());
            double len_width = (node2->GetPos() - node0->GetPos()).Length();

            // Figure out the section for this element
            int b1 = m_num_elements_bead;
            int b2 = m_div_width - m_num_elements_bead;
            int s1 = b1 + m_num_elements_sidewall;
            int s2 = b2 - m_num_elements_sidewall;
            if (j < b1 || j >= b2) {
                // Bead section
                for (unsigned int im = 0; im < m_num_layers_bead; im++) {
                    element->AddLayer(m_layer_thickness_bead[im], CH_C_DEG_TO_RAD * m_ply_angle_bead[im],
                                      m_materials[m_material_id_bead[im]]);
                }
            } else if (j < s1 || j >= s2) {
                // Sidewall section
                for (unsigned int im = 0; im < m_num_layers_sidewall; im++) {
                    element->AddLayer(m_layer_thickness_sidewall[im], CH_C_DEG_TO_RAD * m_ply_angle_sidewall[im],
                                      m_materials[m_material_id_sidewall[im]]);
                }
            } else {
                // Tread section
                for (unsigned int im = 0; im < m_num_layers_tread; im++) {
                    element->AddLayer(m_layer_thickness_tread[im], CH_C_DEG_TO_RAD * m_ply_angle_tread[im],
                                      m_materials[m_material_id_tread[im]]);
                }
            }

            // Set other element properties
            element->SetAlphaDamp(m_alpha);

            // Add element to mesh
            m_mesh->AddElement(element);
        }
    }

    // Switch on automatic gravity
    m_mesh->SetAutomaticGravity(true);
}
예제 #17
0
    static unsigned int BoxBoxContacts(
      Vector&  p_a, ChMatrix33<>& R_a, Vector const & ext_a,
      Vector&  p_b, ChMatrix33<>& R_b, Vector const & ext_b,
      double const & envelope,
      Vector * p,
	  Vector & n,
	  double * distances
      )
    {
      assert(p);
      assert(distances);

	  unsigned int cnt = 0;

      //--- Sign lookup table, could be precomputed!!!
      Vector sign[8];
      for(unsigned int mask=0;mask<8;++mask)
      {
        sign[mask](0) = (mask&0x0001)?1:-1;
        sign[mask](1) = ((mask>>1)&0x0001)?1:-1;
        sign[mask](2) = ((mask>>2)&0x0001)?1:-1;
      }
      //--- extract axis of boxes in WCS
      Vector A[3];
      A[0].x = R_a(0,0);   A[0].y = R_a(1,0);   A[0].z = R_a(2,0);
      A[1].x = R_a(0,1);   A[1].y = R_a(1,1);   A[1].z = R_a(2,1);
      A[2].x = R_a(0,2);   A[2].y = R_a(1,2);   A[2].z = R_a(2,2);
      Vector B[3];
      B[0].x = R_b(0,0);   B[0].y = R_b(1,0);   B[0].z = R_b(2,0);
      B[1].x = R_b(0,1);   B[1].y = R_b(1,1);   B[1].z = R_b(2,1);
      B[2].x = R_b(0,2);   B[2].y = R_b(1,2);   B[2].z = R_b(2,2);

      //--- To compat numerical round-offs, these tend to favor edge-edge
      //--- cases, when one really rather wants a face-case. Truncating
      //--- seems to let the algorithm pick face cases over edge-edge
      //--- cases.
      unsigned int i;

      for( i=0;i<3;++i)
        for(unsigned int j=0;j<3;++j)
        {
          if(fabs(A[i](j))<10e-7)
            A[i](j) = 0.;
          if(fabs(B[i](j))<10e-7)
            B[i](j) = 0.;
        }

      Vector a[8];
      Vector b[8];
      //--- corner points of boxes in WCS
      for( i=0;i<8;++i)
      {
        a[i] = A[2]*(sign[i](2)*ext_a(2)) + A[1]*(sign[i](1)*ext_a(1)) + A[0]*(sign[i](0)*ext_a(0)) + p_a;
        b[i] = B[2]*(sign[i](2)*ext_b(2)) + B[1]*(sign[i](1)*ext_b(1)) + B[0]*(sign[i](0)*ext_b(0)) + p_b;
	  }
      //--- Potential separating axes in WCS
      Vector axis[15];
      axis[0] = A[0];
      axis[1] = A[1];
      axis[2] = A[2];
      axis[3] = B[0];
      axis[4] = B[1];
      axis[5] = B[2];
      axis[6].Cross(A[0],B[0]);
      if(axis[6](0)==0 && axis[6](1)==0 && axis[6](2)==0)
        axis[6] = A[0];
      else
        axis[6] /= sqrt(axis[6].Dot(axis[6]));
      axis[7].Cross(A[0],B[1]);
      if(axis[7](0)==0 && axis[7](1)==0 && axis[7](2)==0)
        axis[7] = A[0];
      else
        axis[7] /= sqrt(axis[7].Dot(axis[7]));
      axis[8].Cross(A[0],B[2]);
      if(axis[8](0)==0 && axis[8](1)==0 && axis[8](2)==0)
        axis[8] = A[0];
      else
        axis[8] /= sqrt(axis[8].Dot(axis[8]));
      axis[9].Cross(A[1],B[0]);
      if(axis[9](0)==0 && axis[9](1)==0 && axis[9](2)==0)
        axis[9] = A[1];
      else
        axis[9] /= sqrt(axis[9].Dot(axis[9]));
      axis[10].Cross(A[1],B[1]);
      if(axis[10](0)==0 && axis[10](1)==0 && axis[10](2)==0)
        axis[10] = A[1];
      else
        axis[10] /= sqrt(axis[10].Dot(axis[10]));
      axis[11].Cross(A[1],B[2]);
      if(axis[11](0)==0 && axis[11](1)==0 && axis[11](2)==0)
        axis[11] = A[1];
      else
        axis[11] /= sqrt(axis[11].Dot(axis[11]));
      axis[12].Cross(A[2],B[0]);
      if(axis[12](0)==0 && axis[12](1)==0 && axis[12](2)==0)
        axis[12] = A[2];
      else
        axis[12] /= sqrt(axis[12].Dot(axis[12]));
      axis[13].Cross(A[2],B[1]);
      if(axis[13](0)==0 && axis[13](1)==0 && axis[13](2)==0)
        axis[13] = A[2];
      else
        axis[13] /= sqrt(axis[13].Dot(axis[13]));
      axis[14].Cross(A[2],B[2]);
      if(axis[14](0)==0 && axis[14](1)==0 && axis[14](2)==0)
        axis[14] = A[2];
      else
        axis[14] /= sqrt(axis[14].Dot(axis[14]));
      //--- project vertices of boxes onto separating axis
      double min_proj_a[15];
      double min_proj_b[15];
      double max_proj_a[15];
      double max_proj_b[15];
      for(i=0;i<15;++i)
      {
        min_proj_a[i] = min_proj_b[i] = 10e30;
        max_proj_a[i] = max_proj_b[i] = -10e30;
      }
      for(i=0;i<15;++i)
      {
        for(unsigned int j=0;j<8;++j)
        {
          double proj_a = a[j].Dot(axis[i]);
          double proj_b = b[j].Dot(axis[i]);
          min_proj_a[i] = ChMin(min_proj_a[i],proj_a);
          max_proj_a[i] = ChMax(max_proj_a[i],proj_a);
          min_proj_b[i] = ChMin(min_proj_b[i],proj_b);
          max_proj_b[i] = ChMax(max_proj_b[i],proj_b);
        }
        //--- test for valid separation axis if so return
        if (min_proj_a[i] > (max_proj_b[i]+envelope) ||   min_proj_b[i] > (max_proj_a[i]+envelope))
          return 0;
      }
      //--- Compute box overlaps along all 15 separating axes, and determine
      //--- minimum overlap
      double overlap[15];
      double minimum_overlap = -10e30;
      unsigned int minimum_axis = 15;
      bool flip_axis[15];
      //--- Notice that edge-edge cases are testet last, so face cases
      //--- are favored over edge-edge cases
      for(i=0;i<15;++i)
      {
        flip_axis[i] = false;
        overlap[i] = 10e30;
        if(max_proj_a[i] <= min_proj_b[i])
        {
          overlap[i] = ChMin( overlap[i], min_proj_b[i] - max_proj_a[i] );
          if(overlap[i]>minimum_overlap)
          {
            minimum_overlap = overlap[i];
            minimum_axis = i;
            flip_axis[i] = false;
          }
        }
        if(max_proj_b[i] <= min_proj_a[i])
        {
          overlap[i] = ChMin( overlap[i], min_proj_a[i] - max_proj_b[i] );
          if(overlap[i]>minimum_overlap)
          {
            minimum_overlap = overlap[i];
            minimum_axis = i;
            flip_axis[i] = true;
          }
        }
        if(min_proj_a[i] <= min_proj_b[i] &&  min_proj_b[i] <= max_proj_a[i])
        {
          overlap[i] = ChMin( overlap[i], -(max_proj_a[i] - min_proj_b[i]) );
          if(overlap[i]>minimum_overlap)
          {
            minimum_overlap = overlap[i];
            minimum_axis = i;
            flip_axis[i] = false;
          }
        }
        if(min_proj_b[i] <= min_proj_a[i] &&  min_proj_a[i] <= max_proj_b[i])
        {
          overlap[i] = ChMin(overlap[i], -(max_proj_b[i] - min_proj_a[i]) );
          if(overlap[i]>minimum_overlap)
          {
            minimum_overlap = overlap[i];
            minimum_axis = i;
            flip_axis[i] = true;
          }
        }
      }
      if(minimum_overlap>envelope)
        return 0;
      //--- Take care of normals, so they point in the correct direction.
      for(i=0;i<15;++i)
      {
        if(flip_axis[i])
          axis[i] = - axis[i];
      }
      //--- At this point we know that a projection along axis[minimum_axis] with
      //--- value minimum_overlap will lead to non-penetration of the two boxes. We
      //--- just need to generate the contact points!!!
      unsigned int corners_inside = 0;
      unsigned int corners_B_in_A = 0;
      unsigned int corners_A_in_B = 0;
      bool AinB[8];
      bool BinA[8];
      Coordsys WCStoA(p_a, R_a.Get_A_quaternion());
	  Coordsys WCStoB(p_b, R_b.Get_A_quaternion());
      Vector eps_a = ext_a + Vector(envelope,envelope,envelope);
      Vector eps_b = ext_b + Vector(envelope,envelope,envelope);
      for(i=0;i<8;++i)
      {
		Vector a_in_B = WCStoB.TransformParentToLocal(a[i]);//ChTransform<>::TransformParentToLocal(a[i], p_a, R_a);
		// = WCStoB.TransformParentToLocal(a[i]);
        Vector abs_a(fabs(a_in_B.x),fabs(a_in_B.y),fabs(a_in_B.z) ) ;
        if(abs_a <= eps_b)
        {
          ++corners_inside;
          ++corners_A_in_B;
          AinB[i] = true;
        }
        else
          AinB[i] = false;
        Vector b_in_A = WCStoA.TransformParentToLocal(b[i]);//= ChTransform<>::TransformParentToLocal(b[i], p_b, R_b);
		// = WCStoA.TransformParentToLocal(b[i]);
        Vector abs_b(fabs(b_in_A.x),fabs(b_in_A.y),fabs(b_in_A.z) );
        if(abs_b <= eps_a)
        {
          ++corners_inside;
          ++corners_B_in_A;
          BinA[i] = true;
        }
        else
          BinA[i] = false;
      }
      //--- This may indicate an edge-edge case
      if(minimum_axis >= 6)
      {
        //--- However the edge-edge case may not be the best choice,
        //--- so if we find a corner point of one box being inside
        //--- the other, we fall back to use the face case with
        //--- minimum overlap.
        if(corners_inside)//--- Actually we only need to test end-points of edge for inclusion (4 points instead of 16!!!).
        {
          minimum_overlap = -10e30;
          minimum_axis = 15;
          for(unsigned int i=0;i<6;++i)
          {
            if(overlap[i]>minimum_overlap)
            {
              minimum_overlap = overlap[i];
              minimum_axis = i;
            }
          }
        }
      }

      //--- now we can safely pick the contact normal, since we
      //--- know wheter we have a face-case or edge-edge case.
      n = axis[minimum_axis];

      //--- This is definitely an edge-edge case
      if(minimum_axis>=6)
      {
        //--- Find a point p_a on the edge from box A.
        for(unsigned int i=0;i<3;++i)
          if(n.Dot(A[i]) > 0.)
            p_a += ext_a(i)*A[i];
          else
            p_a -= ext_a(i)*A[i];
        //--- Find a point p_b on the edge from box B.
        for(int ci=0;ci<3;++ci)
          if(n.Dot(B[ci]) < 0.)
            p_b += ext_b(ci)*B[ci];
          else
            p_b -= ext_b(ci)*B[ci];

        //--- Determine the indices of two unit edge direction vectors (columns
        //--- of rotation matrices in WCS).
        int columnA = ((minimum_axis)-6)/3;
        int columnB = ((minimum_axis)-6)%3;
        double s,t;
        //--- Compute the edge-paramter values s and t corresponding to the closest
        //--- points between the two infinite lines parallel to the two edges.
        ClosestPointsBetweenLines()(p_a,A[columnA],p_b,B[columnB],s,t);
        //--- Use the edge parameter values to compute the closest
        //--- points between the two edges.
        p_a += A[columnA]*s;
        p_b += B[columnB]*t;
        //--- Let the contact point be given by the mean of the closest points.
        p[0] = (p_a + p_b)*.5;
        distances[0] = overlap[minimum_axis];
        return 1;
      }
      //--- This is a face-``something else'' case, we actually already have taken
      //--- care of all corner points, but there might be some edge-edge crossings
      //--- generating contact points



      //--- Make sure that we work in the frame of the box that defines the contact
      //--- normal. This coordinate frame is nice, because the contact-face is a axis
      //--- aligned rectangle. We will refer to this frame as the reference frame, and
      //--- use the letter 'r' or 'R' for it. The other box is named the incident box,
      //--- its closest face towards the reference face is called the incidient face, and
      //--- is denoted by the letter 'i' or 'I'.
      Vector * R_r,* R_i;  //--- Box direction vectors in WCS
      Vector ext_r,ext_i;          //--- Box extents
      Vector p_r,p_i;              //--- Box centers in WCS
      bool * incident_inside;    //--- corner inside state of incident box.
      if (minimum_axis  < 3)
      {
        //--- This means that box A is defining the reference frame
        R_r = A;
        R_i = B;
        p_r = p_a;
        p_i = p_b;
        ext_r = ext_a;
        ext_i = ext_b;
        incident_inside = BinA;
      }
      else
      {
        //--- This means that box B is defining the reference frame
        R_r = B;
        R_i = A;
        p_r = p_b;
        p_i = p_a;
        ext_r = ext_b;
        ext_i = ext_a;
        incident_inside = AinB;
      }
      //--- Following vectors are used for computing the corner points of the incident
      //--- face. At first they are used to determine the axis of the incidient box
      //--- pointing towards the reference box.
      //---
      //--- n_r_wcs = normal pointing away from reference frame in WCS coordinates.
      //--- n_r = normal vector of reference face dotted with axes of incident box.
      //--- abs_n_r = absolute values of n_r.
      Vector n_r_wcs,n_r,abs_n_r;
      if (minimum_axis < 3)
      {
        n_r_wcs = n;
      }
      else
      {
        n_r_wcs = -n;
      }

      //--- Each of these is a measure for how much the axis' of the incident box
      //--- points in the direction of n_r_wcs. The largest absolute value give
      //--- us the axis along which we will find the closest face towards the reference
      //--- box. The sign will tell us if we should take the positive or negative
      //--- face to get the closest incident face.
      n_r(0) = R_i[0].Dot(n_r_wcs);
      n_r(1) = R_i[1].Dot(n_r_wcs);
      n_r(2) = R_i[2].Dot(n_r_wcs);
      abs_n_r(0) = fabs (n_r(0));
      abs_n_r(1) = fabs (n_r(1));
      abs_n_r(2) = fabs (n_r(2));
      //--- Find the largest compontent of abs_n_r: This corresponds to the normal
      //--- for the indident face. The axis number is stored in a3. the other
      //--- axis numbers of the indicent face are stored in a1,a2.
      int a1,a2,a3;
      if (abs_n_r(1) > abs_n_r(0))
      {
        if (abs_n_r(1) > abs_n_r(2))
        {
          a1 = 2; a2 = 0; a3 = 1;
        }
        else
        {
          a1 = 0; a2 = 1; a3 = 2;
        }
      }
      else
      {
        if (abs_n_r(0) > abs_n_r(2))
        {
          a1 = 1; a2 = 2; a3 = 0;
        }
        else
        {
          a1 = 0; a2 = 1; a3 = 2;
        }
      }
      //--- Now we have information enough to determine the incidient face, that means we can
      //--- compute the center point of incident face in WCS coordinates.

      int plus_sign[3];
      Vector center_i_wcs;
      if (n_r(a3) < 0)
      {
        center_i_wcs = p_i + ext_i(a3) * R_i[a3];
        plus_sign[a3] = 1;
      }
      else
      {
        center_i_wcs = p_i - ext_i(a3) * R_i[a3];
        plus_sign[a3] = 0;
      }
      //--- Compute difference of center point of incident face with center of reference coordinates.
      Vector center_ir = center_i_wcs - p_r;
      //--- Find the normal and non-normal axis numbers of the reference box
      int code1,code2,code3;
      if (minimum_axis < 3)
        code3 = minimum_axis;  //012
      else
        code3 = minimum_axis-3;  //345
      if (code3==0)
      {
        code1 = 1;
        code2 = 2;
      }
      else if (code3==1)
      {
        code1 = 2;
        code2 = 0;
      }
      else
      {
        code1 = 0;
        code2 = 1;
      }
      //--- Find the four corners of the incident face, in reference-face coordinates
      double quad[8]; //--- 2D coordinate of incident face (stored as x,y pairs).
      bool inside[4];     //--- inside state of the four coners of the quad
      //--- Project center_ri onto reference-face coordinate system (has origo
      //--- at the center of the reference face, and the two orthogonal unit vectors
      //--- denoted by R_r[code1] and R_r[code2] spaning the face-plane).
      double c1 = R_r[code1].Dot( center_ir);
      double c2 = R_r[code2].Dot( center_ir);
      //--- Compute the projections of the axis spanning the incidient
      //--- face, onto the axis spanning the reference face.
      //---
      //--- This will allow us to determine the coordinates in the reference-face
      //--- when we step along a direction of the incident face given by either
      //--- a1 or a2.
      double m11 = R_r[code1].Dot( R_i[a1]);
      double m12 = R_r[code1].Dot( R_i[a2]);
      double m21 = R_r[code2].Dot( R_i[a1]);
      double m22 = R_r[code2].Dot( R_i[a2]);
      {
        double k1 = m11 * ext_i(a1);
        double k2 = m21 * ext_i(a1);
        double k3 = m12 * ext_i(a2);
        double k4 = m22 * ext_i(a2);

        plus_sign[a1] = 0;
        plus_sign[a2] = 0;
        unsigned int mask = ( (plus_sign[a1]<<a1) |  (plus_sign[a2]<<a2) |  (plus_sign[a3]<<a3));
        inside[0] = incident_inside[ mask ];

        quad[0] = c1 - k1 - k3;
        quad[1] = c2 - k2 - k4;

        plus_sign[a1] = 0;
        plus_sign[a2] = 1;
        mask = (plus_sign[a1]<<a1 |  plus_sign[a2]<<a2 |  plus_sign[a3]<<a3);
        inside[1] = incident_inside[ mask ];

        quad[2] = c1 - k1 + k3;
        quad[3] = c2 - k2 + k4;

        plus_sign[a1] = 1;
        plus_sign[a2] = 1;
        mask = (plus_sign[a1]<<a1 |  plus_sign[a2]<<a2 |  plus_sign[a3]<<a3);
        inside[2] = incident_inside[ mask ];

        quad[4] = c1 + k1 + k3;
        quad[5] = c2 + k2 + k4;

        plus_sign[a1] = 1;
        plus_sign[a2] = 0;
        mask = (plus_sign[a1]<<a1 |  plus_sign[a2]<<a2 |  plus_sign[a3]<<a3);
        inside[3] = incident_inside[ mask ];

        quad[6] = c1 + k1 - k3;
        quad[7] = c2 + k2 - k4;
      }
      //--- find the size of the reference face
      double rect[2];
      rect[0] = ext_r(code1);
      rect[1] = ext_r(code2);

      //--- Intersect the edges of the incident and the reference face
      double crossings[16];
      unsigned int edge_crossings = RectQuadEdgeIntersectionTest()(envelope,rect,quad,inside,crossings);
      assert(edge_crossings<=8);

      if(!corners_inside && !edge_crossings)
        return 0;

      //--- Convert the intersection points into reference-face coordinates,
      //--- and compute the contact position and depth for each point.
      double det1 = 1./(m11*m22 - m12*m21);
      m11 *= det1;
      m12 *= det1;
      m21 *= det1;
      m22 *= det1;

      for (unsigned int j=0; j < edge_crossings; ++j)
      {
        //--- Get coordinates of edge-edge crossing point in reference face coordinate system.
        double p0 = crossings[j*2] - c1;
        double p1 = crossings[j*2+1] - c2;
        //--- Compute intersection point in (almost) WCS. Actually we have
        //--- displaced origin to center of reference frame box
        double k1 =  m22*p0 - m12*p1;
        double k2 = -m21*p0 + m11*p1;
        Vector point = center_ir + k1*R_i[a1] + k2*R_i[a2];
        //--- Depth of intersection point
        double depth = n_r_wcs.Dot(point) - ext_r(code3);
        if(depth<envelope)
        {
          p[cnt] = point + p_r;//--- Move origin from center of reference frame box to WCS
          distances[cnt] = depth;
          ++cnt;
        }
      }
      //      assert((corners_inside + cnt)<=8);//--- If not we are in serious trouble!!!
      //--- I think there is a special case, if corners_inside = 8 and
      //--- corners_in_A = 4 and corners_in_B = 4, then there really
      //--- can only be 4 contacts???

      if(corners_inside)
      {
        unsigned int start_corner_A = cnt;
        unsigned int end_corner_A = cnt;

        //--- Compute Displacement of contact plane from origin of WCS, the
        //--- contact plane is equal to the face plane of the reference box
        double w =   ext_r(code3) +  n_r_wcs.Dot(p_r);

        if(corners_A_in_B)
        {
          for (unsigned int i=0; i < 8; ++i)
          {
            if(AinB[i])
            {
              Vector point = a[i];
              double depth = n_r_wcs.Dot(point) - w;
              if(depth<envelope)
              {
                p[cnt] = point;
                distances[cnt] = depth;
                ++cnt;
              }
            }
          }
          end_corner_A = cnt;
        }
        if(corners_B_in_A)
        {
          for (unsigned int i=0; i < 8; ++i)
          {
            if(BinA[i])
            {
              Vector point = b[i];
              bool redundant = false;
              for(unsigned int j=start_corner_A;j<end_corner_A;++j)
              {
                if( p[j].Equals(point,envelope) )
                {
                  redundant = true;
                  break;
                }
              }
              if(redundant)
                continue;
              double depth = n_r_wcs.Dot(point) - w;
              if(depth<envelope)
              {
                p[cnt] = point;
                distances[cnt] = depth;
                ++cnt;
              }
            }
          }
        }
      }
      //      assert(cnt<=8);//--- If not we are in serious trouble!!!
      return cnt;
    };
예제 #18
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);
    }
}
void ChProximityContainerMeshless::AccumulateStep1()
{
	// Per-edge data computation
	std::list<ChProximityMeshless*>::iterator iterproximity = proximitylist.begin();
	while(iterproximity != proximitylist.end())
	{
		ChMatterMeshless* mmatA = (ChMatterMeshless*)(*iterproximity)->GetModelA()->GetPhysicsItem();
		ChMatterMeshless* mmatB = (ChMatterMeshless*)(*iterproximity)->GetModelB()->GetPhysicsItem();
		ChNodeMeshless* mnodeA =(ChNodeMeshless*) mmatA->GetNode ( ((ChModelBulletNode*)(*iterproximity)->GetModelA())->GetNodeId()  );
		ChNodeMeshless* mnodeB =(ChNodeMeshless*) mmatB->GetNode ( ((ChModelBulletNode*)(*iterproximity)->GetModelB())->GetNodeId()  );
	
		ChVector<> x_A  = mnodeA->GetPos();
		ChVector<> x_B  = mnodeB->GetPos();
		ChVector<> x_Aref  = mnodeA->GetPosReference();
		ChVector<> x_Bref  = mnodeB->GetPosReference();
		ChVector<> u_A = (x_A -x_Aref);
		ChVector<> u_B = (x_B -x_Bref);

		ChVector<> d_BA = x_Bref - x_Aref;
		ChVector<> g_BA = u_B - u_A;
		double dist_BA = d_BA.Length();
		double W_BA =  W_sph( dist_BA, mnodeA->GetKernelRadius() );
		double W_AB =  W_sph( dist_BA, mnodeB->GetKernelRadius() );

		// increment data of connected nodes

		mnodeA->density += mnodeB->GetMass() * W_BA;
		mnodeB->density += mnodeA->GetMass() * W_AB;

		ChMatrixNM<double, 3,1> mdist;
		mdist.PasteVector(d_BA,0,0);
		ChMatrixNM<double, 3,1> mdistT;
		mdistT.PasteVector(d_BA,0,0);
		ChMatrix33<> ddBA; 
		ddBA.MatrMultiplyT(mdist, mdistT);
		ChMatrix33<> ddAB(ddBA);

		ddBA.MatrScale(W_BA);
		mnodeA->Amoment.MatrInc(ddBA);	// increment the moment matrix: Aa += d_BA*d_BA'*W_BA
			
		ddAB.MatrScale(W_AB);
		mnodeB->Amoment.MatrInc(ddAB);	// increment the moment matrix: Ab += d_AB*d_AB'*W_AB


		ChVector<> m_inc_BA = (d_BA)  * W_BA;

		ChVector<> m_inc_AB = (-d_BA) * W_AB;

		ChVector<> dwg;					// increment the J matrix
		dwg = m_inc_BA * g_BA.x;
		mnodeA->J.PasteSumVector(dwg,0,0);
		dwg = m_inc_BA * g_BA.y;
		mnodeA->J.PasteSumVector(dwg,0,1);
		dwg = m_inc_BA * g_BA.z;
		mnodeA->J.PasteSumVector(dwg,0,2);		

		dwg = m_inc_AB * (-g_BA.x);		// increment the J matrix
		mnodeB->J.PasteSumVector(dwg,0,0);
		dwg = m_inc_AB * (-g_BA.y);
		mnodeB->J.PasteSumVector(dwg,0,1);
		dwg = m_inc_AB * (-g_BA.z);
		mnodeB->J.PasteSumVector(dwg,0,2);


		++iterproximity;
	}

}
예제 #20
0
ChVector<double> Angle_to_Angle(AngleSet setfrom, AngleSet setto, const ChVector<double>& mangles) {
    ChVector<double> res;
    ChMatrix33<> Acoord;

    switch (setfrom) {
        case AngleSet::EULERO:
            Acoord.Set_A_Eulero(mangles);
            break;
        case AngleSet::CARDANO:
            Acoord.Set_A_Cardano(mangles);
            break;
        case AngleSet::HPB:
            Acoord.Set_A_Hpb(mangles);
            break;
        case AngleSet::RXYZ:
            Acoord.Set_A_Rxyz(mangles);
            break;
        case AngleSet::RODRIGUEZ:
            Acoord.Set_A_Rodriguez(mangles);
            break;
        default:
            break;
    }

    switch (setto) {
        case AngleSet::EULERO:
            res = Acoord.Get_A_Eulero();
            break;
        case AngleSet::CARDANO:
            res = Acoord.Get_A_Cardano();
            break;
        case AngleSet::HPB:
            res = Acoord.Get_A_Hpb();
            break;
        case AngleSet::RXYZ:
            res = Acoord.Get_A_Rxyz();
            break;
        case AngleSet::RODRIGUEZ:
            res = Acoord.Get_A_Rodriguez();
            break;
        default:
            break;
    }
    return res;
}
예제 #21
0
bool CHOBB::OBB_Overlap(ChMatrix33<>& B, Vector T, Vector a, Vector b)
{
	  register double t, s;
	  register int r;
	  static ChMatrix33<> Bf;
	  const double reps = (double)1e-6;

	  //Vector& a = b1->d;
	  //Vector& b = b2->d;

	  // Bf = fabs(B)
	  Bf(0,0) = myfabs(B.Get33Element(0,0));  Bf(0,0) += reps;
	  Bf(0,1) = myfabs(B.Get33Element(0,1));  Bf(0,1) += reps;
	  Bf(0,2) = myfabs(B.Get33Element(0,2));  Bf(0,2) += reps;
	  Bf(1,0) = myfabs(B.Get33Element(1,0));  Bf(1,0) += reps;
	  Bf(1,1) = myfabs(B.Get33Element(1,1));  Bf(1,1) += reps;
	  Bf(1,2) = myfabs(B.Get33Element(1,2));  Bf(1,2) += reps;
	  Bf(2,0) = myfabs(B.Get33Element(2,0));  Bf(2,0) += reps;
	  Bf(2,1) = myfabs(B.Get33Element(2,1));  Bf(2,1) += reps;
	  Bf(2,2) = myfabs(B.Get33Element(2,2));  Bf(2,2) += reps;

	  // if any of these tests are one-sided, then the polyhedra are disjoint
	  r = 1;

	  // A1 x A2 = A0
	  t = myfabs(T.x);

	  r &= (t <=
		  (a.x + b.x * Bf.Get33Element(0,0) + b.y * Bf.Get33Element(0,1) + b.z * Bf.Get33Element(0,2)));
	  if (!r) return false;

	  // B1 x B2 = B0
	  s = T.x*B.Get33Element(0,0) + T.y*B.Get33Element(1,0) + T.z*B.Get33Element(2,0);
	  t = myfabs(s);

	  r &= ( t <=
		  (b.x + a.x * Bf.Get33Element(0,0) + a.y * Bf.Get33Element(1,0) + a.z * Bf.Get33Element(2,0)));
	  if (!r) return false;

	  // A2 x A0 = A1
	  t = myfabs(T.y);

	  r &= ( t <=
		  (a.y + b.x * Bf.Get33Element(1,0) + b.y * Bf.Get33Element(1,1) + b.z * Bf.Get33Element(1,2)));
	  if (!r) return false;

	  // A0 x A1 = A2
	  t = myfabs(T.z);

	  r &= ( t <=
		  (a.z + b.x * Bf.Get33Element(2,0) + b.y * Bf.Get33Element(2,1) + b.z * Bf.Get33Element(2,2)));
	  if (!r) return false;

	  // B2 x B0 = B1
	  s = T.x*B.Get33Element(0,1) + T.y*B.Get33Element(1,1) + T.z*B.Get33Element(2,1);
	  t = myfabs(s);

	  r &= ( t <=
		  (b.y + a.x * Bf.Get33Element(0,1) + a.y * Bf.Get33Element(1,1) + a.z * Bf.Get33Element(2,1)));
	  if (!r) return false;

	  // B0 x B1 = B2
	  s = T.x*B.Get33Element(0,2) + T.y*B.Get33Element(1,2) + T.z*B.Get33Element(2,2);
	  t = myfabs(s);

	  r &= ( t <=
		  (b.z + a.x * Bf.Get33Element(0,2) + a.y * Bf.Get33Element(1,2) + a.z * Bf.Get33Element(2,2)));
	  if (!r) return false;

	  // A0 x B0
	  s = T.z * B.Get33Element(1,0) - T.y * B.Get33Element(2,0);
	  t = myfabs(s);

	  r &= ( t <=
		(a.y * Bf.Get33Element(2,0) + a.z * Bf.Get33Element(1,0) +
		 b.y * Bf.Get33Element(0,2) + b.z * Bf.Get33Element(0,1)));
	  if (!r) return false;

	  // A0 x B1
	  s = T.z * B.Get33Element(1,1) - T.y * B.Get33Element(2,1);
	  t = myfabs(s);

	  r &= ( t <=
		(a.y * Bf.Get33Element(2,1) + a.z * Bf.Get33Element(1,1) +
		 b.x * Bf.Get33Element(0,2) + b.z * Bf.Get33Element(0,0)));
	  if (!r) return false;

	  // A0 x B2
	  s = T.z * B.Get33Element(1,2) - T.y * B.Get33Element(2,2);
	  t = myfabs(s);

	  r &= ( t <=
		  (a.y * Bf.Get33Element(2,2) + a.z * Bf.Get33Element(1,2) +
		   b.x * Bf.Get33Element(0,1) + b.y * Bf.Get33Element(0,0)));
	  if (!r) return false;

	  // A1 x B0
	  s = T.x * B.Get33Element(2,0) - T.z * B.Get33Element(0,0);
	  t = myfabs(s);

	  r &= ( t <=
		  (a.x * Bf.Get33Element(2,0) + a.z * Bf.Get33Element(0,0) +
		   b.y * Bf.Get33Element(1,2) + b.z * Bf.Get33Element(1,1)));
	  if (!r) return false;

	  // A1 x B1
	  s = T.x * B.Get33Element(2,1) - T.z * B.Get33Element(0,1);
	  t = myfabs(s);

	  r &= ( t <=
		  (a.x * Bf.Get33Element(2,1) + a.z * Bf.Get33Element(0,1) +
		   b.x * Bf.Get33Element(1,2) + b.z * Bf.Get33Element(1,0)));
	  if (!r) return false;

	  // A1 x B2
	  s = T.x * B.Get33Element(2,2) - T.z * B.Get33Element(0,2);
	  t = myfabs(s);

	  r &= (t <=
		  (a.x * Bf.Get33Element(2,2) + a.z * Bf.Get33Element(0,2) +
		   b.x * Bf.Get33Element(1,1) + b.y * Bf.Get33Element(1,0)));
	  if (!r) return false;

	  // A2 x B0
	  s = T.y * B.Get33Element(0,0) - T.x * B.Get33Element(1,0);
	  t = myfabs(s);

	  r &= (t <=
		  (a.x * Bf.Get33Element(1,0) + a.y * Bf.Get33Element(0,0) +
		   b.y * Bf.Get33Element(2,2) + b.z * Bf.Get33Element(2,1)));
	  if (!r) return false;

	  // A2 x B1
	  s = T.y * B.Get33Element(0,1) - T.x * B.Get33Element(1,1);
	  t = myfabs(s);

	  r &= ( t <=
		  (a.x * Bf.Get33Element(1,1) + a.y * Bf.Get33Element(0,1) +
		   b.x * Bf.Get33Element(2,2) + b.z * Bf.Get33Element(2,0)));
	  if (!r) return false;

	  // A2 x B2
	  s = T.y * B.Get33Element(0,2) - T.x * B.Get33Element(1,2);
	  t = myfabs(s);

	  r &= ( t <=
		  (a.x * Bf.Get33Element(1,2) + a.y * Bf.Get33Element(0,2) +
		   b.x * Bf.Get33Element(2,1) + b.y * Bf.Get33Element(2,0)));
	  if (!r) return false;


	  return true;  // no separation: BV collide!!
}