C++ ChMatrix33类代码示例

// -----------------------------------------------------------------------------
// 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());
}

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;
}

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;
}

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;
}

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;
}

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 
//.........这里部分代码省略.........

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);
}

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;
	}
}

// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
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);
//.........这里部分代码省略.........

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:
//.........这里部分代码省略.........

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
//.........这里部分代码省略.........

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);

//.........这里部分代码省略.........

    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)
//.........这里部分代码省略.........