C++ ChQuaternion类代码示例

///	Get the quaternion second derivative from the vector of angular acceleration with a specified in _relative_ coords.
ChQuaternion<double> Qdtdt_from_Arel(const ChVector<double>& a,
                                     const ChQuaternion<double>& q,
                                     const ChQuaternion<double>& q_dt) {
    ChQuaternion<double> ret;
    ret.Qdtdt_from_Arel(a, q, q_dt);
    return ret;
}

// Get the quaternion time derivative from the vector of angular speed, with w specified in _local_ coords.
ChQuaternion<double> Qdt_from_Wrel(const ChVector<double>& w, const ChQuaternion<double>& q) {
    ChQuaternion<double> qw;
    double half = 0.5;

    qw.e0() = 0;
    qw.e1() = w.x();
    qw.e2() = w.y();
    qw.e3() = w.z();

    return Qscale(Qcross(q, qw), half);  // {q_dt} = 1/2 {q}*{0,w_rel}
}

ChQuaternion<double> Qscale(const ChQuaternion<double>& q, double fact) {
    ChQuaternion<double> result;
    result.e0() = q.e0() * fact;
    result.e1() = q.e1() * fact;
    result.e2() = q.e2() * fact;
    result.e3() = q.e3() * fact;
    return result;
}

// Return the conjugate of the quaternion [s,v1,v2,v3] is [s,-v1,-v2,-v3]
ChQuaternion<double> Qconjugate(const ChQuaternion<double>& q) {
    ChQuaternion<double> res;
    res.e0() = q.e0();
    res.e1() = -q.e1();
    res.e2() = -q.e2();
    res.e3() = -q.e3();
    return (res);
}

// Get the X axis of a coordsystem, given the quaternion which
// represents the alignment of the coordsystem.
ChVector<double> VaxisXfromQuat(const ChQuaternion<double>& quat) {
    ChVector<double> res;
    res.x() = (pow(quat.e0(), 2) + pow(quat.e1(), 2)) * 2 - 1;
    res.y() = ((quat.e1() * quat.e2()) + (quat.e0() * quat.e3())) * 2;
    res.z() = ((quat.e1() * quat.e3()) - (quat.e0() * quat.e2())) * 2;
    return res;
}

// Get the quaternion from an angle of rotation and an axis, defined in _abs_ coords.
// The axis is supposed to be fixed, i.e. it is constant during rotation.
// The 'axis' vector must be normalized.
ChQuaternion<double> Q_from_AngAxis(double angle, const ChVector<double>& axis) {
    ChQuaternion<double> quat;
    double halfang;
    double sinhalf;

    halfang = (angle * 0.5);
    sinhalf = sin(halfang);

    quat.e0() = cos(halfang);
    quat.e1() = axis.x() * sinhalf;
    quat.e2() = axis.y() * sinhalf;
    quat.e3() = axis.z() * sinhalf;
    return (quat);
}

// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
void ChTrackShoeBandBushing::Initialize(std::shared_ptr<ChBodyAuxRef> chassis,
                                        const ChVector<>& location,
                                        const ChQuaternion<>& rotation) {
    // Initialize base class (create tread body)
    ChTrackShoeBand::Initialize(chassis, location, rotation);

    // Cache values calculated from template parameters.
    m_seg_length = GetWebLength() / GetNumWebSegments();
    m_seg_mass = GetWebMass() / GetNumWebSegments();
    m_seg_inertia = GetWebInertia();  //// TODO - properly distribute web inertia

    // Express the tread body location and orientation in global frame.
    ChVector<> loc = chassis->TransformPointLocalToParent(location);
    ChQuaternion<> rot = chassis->GetRot() * rotation;
    ChVector<> xdir = rot.GetXaxis();

    // Create the required number of web segment bodies
    ChVector<> seg_loc = loc + (0.5 * GetToothBaseLength()) * xdir;
    for (int is = 0; is < GetNumWebSegments(); is++) {
        m_web_segments.push_back(std::shared_ptr<ChBody>(chassis->GetSystem()->NewBody()));
        m_web_segments[is]->SetNameString(m_name + "_web_" + std::to_string(is));
        m_web_segments[is]->SetPos(seg_loc + ((2 * is + 1) * m_seg_length / 2) * xdir);
        m_web_segments[is]->SetRot(rot);
        m_web_segments[is]->SetMass(m_seg_mass);
        m_web_segments[is]->SetInertiaXX(m_seg_inertia);
        chassis->GetSystem()->AddBody(m_web_segments[is]);

        // Add contact geometry.
        m_web_segments[is]->SetCollide(true);

        switch (m_web_segments[is]->GetContactMethod()) {
            case ChMaterialSurface::NSC:
                m_web_segments[is]->GetMaterialSurfaceNSC()->SetFriction(m_friction);
                m_web_segments[is]->GetMaterialSurfaceNSC()->SetRestitution(m_restitution);
                break;
            case ChMaterialSurface::SMC:
                m_web_segments[is]->GetMaterialSurfaceSMC()->SetFriction(m_friction);
                m_web_segments[is]->GetMaterialSurfaceSMC()->SetRestitution(m_restitution);
                m_web_segments[is]->GetMaterialSurfaceSMC()->SetYoungModulus(m_young_modulus);
                m_web_segments[is]->GetMaterialSurfaceSMC()->SetPoissonRatio(m_poisson_ratio);
                m_web_segments[is]->GetMaterialSurfaceSMC()->SetKn(m_kn);
                m_web_segments[is]->GetMaterialSurfaceSMC()->SetGn(m_gn);
                m_web_segments[is]->GetMaterialSurfaceSMC()->SetKt(m_kt);
                m_web_segments[is]->GetMaterialSurfaceSMC()->SetGt(m_gt);
                break;
        }

        AddWebContact(m_web_segments[is]);
    }
}

void System::DoTimeStep() {
	if (mNumCurrentObjects < mNumObjects && mFrameNumber % 100 == 0) {
		float x = 1, y = numY, z = 1;
		float posX = 0, posY = -8, posZ = 0;
		srand(1);
		float mass = .01, mu = .5, rest = 0;
		ShapeType type = SPHERE;
		CHBODYSHAREDPTR mrigidBody;
		mNumCurrentObjects += x * y * z;
		int mobjNum = 0;
		for (int xx = 0; xx < x; xx++) {
			for (int yy = 0; yy < y; yy++) {
				for (int zz = 0; zz < z; zz++) {
					type = CYLINDER;//rand()%2;e
					float radius = .5;//(rand()%1000)/3000.0+.05;
					ChVector<> mParticlePos((xx - (x - 1) / 2.0) + posX, (yy) + posY, (zz - (z - 1) / 2.0) + posZ);

					//mParticlePos += ChVector<> (rand() % 1000 / 10000.0 - .05, rand() % 1000 / 10000.0 - .05, rand() % 1000 / 10000.0 - .05);
					ChQuaternion<> quat = ChQuaternion<> (1, 0, 0, 0);// (rand() % 1000 / 1000., rand() % 1000 / 1000., rand() % 1000 / 1000., rand() % 1000 / 1000.);
					ChVector<> dim;
					ChVector<> lpos(0, 0, 0);
					quat.Normalize();

					mrigidBody = CHBODYSHAREDPTR(new CHBODY);
					InitObject(mrigidBody, mass, mParticlePos * 1.1, quat, mu, mu, rest, true, false, 0, 1);
					mrigidBody->SetPos_dt(ChVector<> (0, 0, 0));
					switch (type) {
						case SPHERE:
							dim = ChVector<> (radius, 0, 0);
						case ELLIPSOID:
							dim = ChVector<> (radius * 1.3, radius, radius * 1.3);
						case BOX:
							dim = ChVector<> (radius, radius, radius);
						case CYLINDER:
							dim = ChVector<> (radius, radius*2, radius);
					}
					AddCollisionGeometry(mrigidBody, type, dim, lpos, quat);
					FinalizeObject(mrigidBody);
					mobjNum++;
				}
			}
		}
	}
	mFrameNumber++;
	mSystem->DoStepDynamics(mTimeStep);
	mCurrentTime += mTimeStep;
	GPUSystem->PrintStats();
}

ChQuaternion<double> Q_from_NasaAngles(const ChVector<double>& mang) {
    ChQuaternion<double> mq;
    double c1 = cos(mang.z() / 2);
    double s1 = sin(mang.z() / 2);
    double c2 = cos(mang.x() / 2);
    double s2 = sin(mang.x() / 2);
    double c3 = cos(mang.y() / 2);
    double s3 = sin(mang.y() / 2);
    double c1c2 = c1 * c2;
    double s1s2 = s1 * s2;
    mq.e0() = c1c2 * c3 + s1s2 * s3;
    mq.e1() = c1c2 * s3 - s1s2 * c3;
    mq.e2() = c1 * s2 * c3 + s1 * c2 * s3;
    mq.e3() = s1 * c2 * c3 - c1 * s2 * s3;
    return mq;
}

void Q_to_AngAxis(const ChQuaternion<double>& quat, double& angle, ChVector<double>& axis) {
    if (fabs(quat.e0()) < 0.99999999) {
        double arg = acos(quat.e0());
        double invsine = 1 / sin(arg);
        ChVector<double> vtemp;
        vtemp.x() = invsine * quat.e1();
        vtemp.y() = invsine * quat.e2();
        vtemp.z() = invsine * quat.e3();
        angle = 2 * arg;
        axis = Vnorm(vtemp);
    } else {
        axis.x() = 1;
        axis.y() = 0;
        axis.z() = 0;
        angle = 0;
    }
}

    // Use convex decomposition for collision detection with the Trelleborg tire
    SoilbinWheel(ChSystemNSC* system, ChVector<> mposition, double mass, ChVector<>& inertia) {
        ChCollisionModel::SetDefaultSuggestedEnvelope(0.005);
        ChCollisionModel::SetDefaultSuggestedMargin(0.004);

        // Create the wheel body
        wheel = std::make_shared<ChBody>();
        wheel->SetPos(mposition);
        wheel->SetMass(mass);
        wheel->SetInertiaXX(inertia);
        wheel->GetMaterialSurfaceNSC()->SetFriction(0.4f);
        wheel->SetCollide(true);

        // Visualization mesh
        auto tireMesh = std::make_shared<ChTriangleMeshConnected>();
        tireMesh->LoadWavefrontMesh(GetChronoDataFile("tractor_wheel.obj"), true, true);
        auto tireMesh_asset = std::make_shared<ChTriangleMeshShape>();
        tireMesh_asset->SetMesh(tireMesh);
        wheel->AddAsset(tireMesh_asset);

        // Contact mesh
        wheel->GetCollisionModel()->ClearModel();
        // Describe the (invisible) colliding shape by adding the 'carcass' decomposed shape and the
        // 'knobs'. Since these decompositions are only for 1/15th of the wheel, use for() to pattern them.
        for (double mangle = 0; mangle < 360.; mangle += (360. / 15.)) {
            ChQuaternion<> myrot;
            ChStreamInAsciiFile myknobs(GetChronoDataFile("tractor_wheel_knobs.chulls").c_str());
            ChStreamInAsciiFile myslice(GetChronoDataFile("tractor_wheel_slice.chulls").c_str());
            myrot.Q_from_AngAxis(mangle * (CH_C_PI / 180.), VECT_X);
            ChMatrix33<> mm(myrot);
            wheel->GetCollisionModel()->AddConvexHullsFromFile(myknobs, ChVector<>(0, 0, 0), mm);
            wheel->GetCollisionModel()->AddConvexHullsFromFile(myslice, ChVector<>(0, 0, 0), mm);
        }
        wheel->GetCollisionModel()->BuildModel();

        // Add wheel body to system
        system->AddBody(wheel);
    }

ChQuaternion<double> Qsub(const ChQuaternion<double>& qa, const ChQuaternion<double>& qb) {
    ChQuaternion<double> result;
    result.e0() = qa.e0() - qb.e0();
    result.e1() = qa.e1() - qb.e1();
    result.e2() = qa.e2() - qb.e2();
    result.e3() = qa.e3() - qb.e3();
    return result;
}

// Given the imaginary (vectorial) {e1 e2 e3} part of a quaternion second time derivative,
// find the entire quaternion q = {e0, e1, e2, e3}.
// Note: singularities are possible.
ChQuaternion<double> ImmQ_dtdt_complete(const ChQuaternion<double>& mq,
                                        const ChQuaternion<double>& mqdt,
                                        const ChVector<double>& qimm_dtdt) {
    ChQuaternion<double> mqdtdt;
    mqdtdt.e1() = qimm_dtdt.x();
    mqdtdt.e2() = qimm_dtdt.y();
    mqdtdt.e3() = qimm_dtdt.z();
    mqdtdt.e0() = (-mq.e1() * mqdtdt.e1() - mq.e2() * mqdtdt.e2() - mq.e3() * mqdtdt.e3() - mqdt.e0() * mqdt.e0() -
                   mqdt.e1() * mqdt.e1() - mqdt.e2() * mqdt.e2() - mqdt.e3() * mqdt.e3()) /
                  mq.e0();
    return mqdtdt;
}

ChVector<double> Q_to_NasaAngles(const ChQuaternion<double>& q1) {
    ChVector<double> mnasa;
    double sqw = q1.e0() * q1.e0();
    double sqx = q1.e1() * q1.e1();
    double sqy = q1.e2() * q1.e2();
    double sqz = q1.e3() * q1.e3();
    // heading
    mnasa.z() = atan2(2.0 * (q1.e1() * q1.e2() + q1.e3() * q1.e0()), (sqx - sqy - sqz + sqw));
    // bank
    mnasa.y() = atan2(2.0 * (q1.e2() * q1.e3() + q1.e1() * q1.e0()), (-sqx - sqy + sqz + sqw));
    // attitude
    mnasa.x() = asin(-2.0 * (q1.e1() * q1.e3() - q1.e2() * q1.e0()));
    return mnasa;
}

// Get the quaternion from a source vector and a destination vector which specifies
// the rotation from one to the other.  The vectors do not need to be normalized.
ChQuaternion<double> Q_from_Vect_to_Vect(const ChVector<double>& fr_vect, const ChVector<double>& to_vect) {
    const double ANGLE_TOLERANCE = 1e-6;
    ChQuaternion<double> quat;
    double halfang;
    double sinhalf;
    ChVector<double> axis;

    double lenXlen = fr_vect.Length() * to_vect.Length();
    axis = fr_vect % to_vect;
    double sinangle = ChClamp(axis.Length() / lenXlen, -1.0, +1.0);
    double cosangle = ChClamp(fr_vect ^ to_vect / lenXlen, -1.0, +1.0);

    // Consider three cases: Parallel, Opposite, non-collinear
    if (std::abs(sinangle) == 0.0 && cosangle > 0) {
        // fr_vect & to_vect are parallel
        quat.e0() = 1.0;
        quat.e1() = 0.0;
        quat.e2() = 0.0;
        quat.e3() = 0.0;
    } else if (std::abs(sinangle) < ANGLE_TOLERANCE && cosangle < 0) {
        // fr_vect & to_vect are opposite, i.e. ~180 deg apart
        axis = fr_vect.GetOrthogonalVector() + (-to_vect).GetOrthogonalVector();
        axis.Normalize();
        quat.e0() = 0.0;
        quat.e1() = ChClamp(axis.x(), -1.0, +1.0);
        quat.e2() = ChClamp(axis.y(), -1.0, +1.0);
        quat.e3() = ChClamp(axis.z(), -1.0, +1.0);
    } else {
        // fr_vect & to_vect are not co-linear case
        axis.Normalize();
        halfang = 0.5 * ChAtan2(sinangle, cosangle);
        sinhalf = sin(halfang);

        quat.e0() = cos(halfang);
        quat.e1() = ChClamp(axis.x(), -1.0, +1.0);
        quat.e2() = ChClamp(axis.y(), -1.0, +1.0);
        quat.e3() = ChClamp(axis.z(), -1.0, +1.0);
    }
    return (quat);
}