// -----------------------------------------------------------------------------
// 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;
}
// -----------------------------------------------------------------------------
// 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;
}
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
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";
*/
}
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);
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);
}
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;
}
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;
}
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
}
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);
}
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;
};
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;
}
}
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;
}
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!!
}