public ChVector GetNormal()
{
ChVector mn = new ChVector();
Normal(ref mn);
return(mn);
}
/// Add a box shape to this model, for collision purposes
public abstract bool AddBox(
double hx, /// the halfsize on x axis
double hy, /// the halfsize on y axis
double hz, /// the halfsize on z axis
ChVector pos, /// the position of the box COG
ChMatrix33 <double> rot /// the rotation of the box - matrix must be orthogonal
);
public bool FindPoint(double x, double y, out double height, ref ChVector normal, float friction)
{
bool hit = false;
height = -1000;
normal = new ChVector(0, 1, 0);
friction = 0.8f;
ChVector from = new ChVector(x, 1000, y);
ChVector to = new ChVector(x, -1000, y);
for (int i = 0; i < patches.Count; i++)
{
// ChBody body = patch.GetComponent<ChBody>();
//collision.ChCollisionSystem.ChRayhitResult result = new collision.ChCollisionSystem.ChRayhitResult();
ChSystem.system.GetCollisionSystem().RayHit(from, to, patches[i].GetCollisionModel(), ref result);
if (result.hit && result.abs_hitPoint.y > height)
{
hit = true;
height = result.abs_hitPoint.y;
normal = result.abs_hitNormal;
//friction = patch.m_friction;
}
}
return(hit);
}
/// Perform disc-terrain collision detection.
/// This utility function checks for contact between a disc of specified
/// radius with given position and orientation (specified as the location of
/// its center and a unit vector normal to the disc plane) and the terrain
/// system associated with this tire. It returns true if the disc contacts the
/// terrain and false otherwise. If contact occurs, it returns a coordinate
/// system with the Z axis along the contact normal and the X axis along the
/// "rolling" direction, as well as a positive penetration depth (i.e. the
/// height below the terrain of the lowest point on the disc).
protected static bool disc_terrain_contact(
ChTerrain terrain, ///< [in] reference to terrain system
ChVector disc_center, ///< [in] global location of the disc center
ChVector disc_normal, ///< [in] disc normal, expressed in the global frame
double disc_radius, ///< [in] disc radius
ref ChCoordsys contact, ///< [out] contact coordinate system (relative to the global frame)
ref double depth ///< [out] penetration depth (positive if contact occurred)
)
{
// 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 nhelp = terrain.GetNormal(disc_center.x, disc_center.y);
ChVector dir1 = ChVector.Vcross(disc_normal, nhelp);
double sinTilt2 = dir1.Length2();
if (sinTilt2 < 1e-3)
{
return(false);
}
// Contact point (lowest point on disc).
ChVector ptD = disc_center + disc_radius * ChVector.Vcross(disc_normal, dir1 / Math.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 = ChVector.Vcross(disc_normal, normal);
longitudinal.Normalize();
ChVector lateral = ChVector.Vcross(normal, longitudinal);
ChMatrix33 <double> rot = new ChMatrix33 <double>(0); // Need to nest this.
rot.Set_A_axis(longitudinal, lateral, normal);
contact.pos = ptD;
contact.rot = rot.Get_A_quaternion();
depth = ChVector.Vdot(new ChVector(0, 0, hp - ptD.z), normal);
//assert(depth > 0);
return(true);
}
// compute triangle normal
public bool Normal(ref ChVector N)
{
ChVector u;
u = ChVector.Vsub(p2, p1);
ChVector v;
v = ChVector.Vsub(p3, p1);
ChVector n;
n = ChVector.Vcross(u, v);
double len = ChVector.Vlength(n);
if (Mathfx.Abs(len) > EPS_TRIDEGENERATE)
{
N = ChVector.Vmul(n, (1.0 / len));
}
else
{
return(false);
}
return(true);
}
public override void GetBoundingBox(ref double xmin,
ref double xmax,
ref double ymin,
ref double ymax,
ref double zmin,
ref double zmax,
ChMatrix33 <double> Rot)
{
if (Rot == null)
{
xmin = ChMaths.ChMin(ChMaths.ChMin(p1.x, p2.x), p3.x);
ymin = ChMaths.ChMin(ChMaths.ChMin(p1.y, p2.y), p3.y);
zmin = ChMaths.ChMin(ChMaths.ChMin(p1.z, p2.z), p3.z);
xmax = ChMaths.ChMax(ChMaths.ChMax(p1.x, p2.x), p3.x);
ymax = ChMaths.ChMax(ChMaths.ChMax(p1.y, p2.y), p3.y);
zmax = ChMaths.ChMax(ChMaths.ChMax(p1.z, p2.z), p3.z);
}
else
{
ChVector trp1 = Rot.MatrT_x_Vect(p1);
ChVector trp2 = Rot.MatrT_x_Vect(p2);
ChVector trp3 = Rot.MatrT_x_Vect(p3);
xmin = ChMaths.ChMin(ChMaths.ChMin(trp1.x, trp2.x), trp3.x);
ymin = ChMaths.ChMin(ChMaths.ChMin(trp1.y, trp2.y), trp3.y);
zmin = ChMaths.ChMin(ChMaths.ChMin(trp1.z, trp2.z), trp3.z);
xmax = ChMaths.ChMax(ChMaths.ChMax(trp1.x, trp2.x), trp3.x);
ymax = ChMaths.ChMax(ChMaths.ChMax(trp1.y, trp2.y), trp3.y);
zmax = ChMaths.ChMax(ChMaths.ChMax(trp1.z, trp2.z), trp3.z);
}
}
/// Add a triangle from mesh.
/// For efficiency, points are stored as pointers. Thus, the user must
/// take care of memory management and of dangling pointers.
public virtual bool AddTriangleProxy(ChVector p1, //< points to vertex1 coords
ChVector p2, //< points to vertex2 coords
ChVector p3, //< points to vertex3 coords
ChVector ep1, //< points to neighbouring vertex at edge1 if any
ChVector ep2, //< points to neighbouring vertex at edge1 if any
ChVector ep3, //< points to neighbouring vertex at edge1 if any
bool mowns_vertex_1, //< vertex is owned by this triangle (otherwise, owned by neighbour)
bool mowns_vertex_2, //< vertex is owned by this triangle (otherwise, owned by neighbour)
bool mowns_vertex_3, //< vertex is owned by this triangle (otherwise, owned by neighbour)
bool mowns_edge_1, //< edge is owned by this triangle (otherwise, owned by neighbour)
bool mowns_edge_2, //< edge is owned by this triangle (otherwise, owned by neighbour)
bool mowns_edge_3, //< edge is owned by this triangle (otherwise, owned by neighbour)
double msphereswept_rad = 0 //< sphere swept triangle ('fat' triangle, improves robustness)
)
{
// adjust default inward 'safe' margin (always as radius)
this.SetSafeMargin(msphereswept_rad);
btCEtriangleShape mshape = new btCEtriangleShape(p1, p2, p3, ep1, ep2, ep3,
mowns_vertex_1, mowns_vertex_2, mowns_vertex_3,
mowns_edge_1, mowns_edge_2, mowns_edge_3, msphereswept_rad);
mshape.SetMargin((float)this.GetSuggestedFullMargin()); // this.GetSafeMargin()); // not this.GetSuggestedFullMargin() given the way that btCEtriangleShape works.
_injectShape(ChVector.VNULL, new ChMatrix33 <double>(1), mshape);
return(true);
}
/// Perform a ray-hit test with the collision models.
public override bool RayHit(ChVector from,
ChVector to,
ChCollisionModel model,
ref ChRayhitResult mresult)
{
return(RayHit(from, to, model, ref mresult, CollisionFilterGroups.DefaultFilter, CollisionFilterGroups.AllFilter));
}
/// Calculate distance between a point p and a line identified
/// with segment dA,dB. Returns distance. Also, the mu value reference
/// tells if the nearest projection of point on line falls into segment (for mu 0...1)
/// \return the distance
public static double PointLineDistance(
ref ChVector p, //< point to be measured
ref ChVector dA, //< a point on the line
ref ChVector dB, //< another point on the line
ref double mu, //< parametric coord: if in 0..1 interval, projection is between dA and dB
ref bool is_insegment //< returns true if projected point is between dA and dB
)
{
mu = -1.0;
is_insegment = false;
double mdist = 10e34;
ChVector vseg = ChVector.Vsub(dB, dA);
ChVector vdir = ChVector.Vnorm(vseg);
ChVector vray = ChVector.Vsub(p, dA);
mdist = ChVector.Vlength(ChVector.Vcross(vray, vdir));
mu = ChVector.Vdot(vray, vdir) / ChVector.Vlength(vseg);
if ((mu >= 0) && (mu <= 1.0))
{
is_insegment = true;
}
return(mdist);
}
/// Add a cylinder to this model (default axis on Y direction), for collision purposes
/*public abstract bool AddCylinder(double rx,
* double rz,
* double hy
* //ChVector<>& pos = ChVector<>(),
* //ChMatrix33<>& rot = ChMatrix33<>(1)) = 0;
* );*/
/// Add a triangle mesh to this model, passing a triangle mesh.
/// Note: if possible, for better performance, avoid triangle meshes and prefer simplified
/// representations as compounds of primitive convex shapes (boxes, sphers, etc).
public abstract bool AddTriangleMesh( //
Mesh trimesh, //< the triangle mesh
bool is_static, //< true if model doesn't move. May improve performance.
bool is_convex, //< if true, a convex hull is used. May improve robustness.
ChVector pos, //< displacement respect to COG
ChMatrix33 <double> rot, //< the rotation of the mesh
double sphereswept_thickness = 0.0 //< outward sphere-swept layer (when supported)
);
private void _injectShape(ChVector pos, ChMatrix33 <double> rot, CollisionShape mshape)
{
bool centered = true;// (pos.IsNull() && rot.IsIdentity()); // FIX THIS !!!
// This is needed so later one can access ChModelBullet::GetSafeMargin and ChModelBullet::GetEnvelope
mshape.SetUserPointer(this);
// start_vector = || -- description is still empty
if (shapes.Count == 0)
{
if (centered)
{
shapes.Add(mshape);
bt_collision_object.SetCollisionShape(mshape);
// end_vector= | centered shape |
return;
}
else
{
CompoundShape mcompound = new CompoundShape();
shapes.Add(mcompound);
shapes.Add(mshape);
bt_collision_object.SetCollisionShape(mcompound);
IndexedMatrix mtransform = new IndexedMatrix();
ChPosMatrToBullet(pos, rot, ref mtransform);
mcompound.AddChildShape(ref mtransform, mshape);
// vector= | compound | not centered shape |
return;
}
}
// start_vector = | centered shape | ----just a single centered shape was added
if (shapes.Count == 1)
{
IndexedMatrix mtransform = new IndexedMatrix();
shapes.Add(shapes[0]);
shapes.Add(mshape);
CompoundShape mcompound = new CompoundShape(true);
shapes[0] = mcompound;
bt_collision_object.SetCollisionShape(mcompound);
//mtransform.setIdentity();
mcompound.AddChildShape(ref mtransform, shapes[1]);
ChPosMatrToBullet(pos, rot, ref mtransform);
mcompound.AddChildShape(ref mtransform, shapes[2]);
// vector= | compound | old centered shape | new shape | ...
return;
}
// vector= | compound | old | old.. | ----already working with compounds..
if (shapes.Count > 1)
{
IndexedMatrix mtransform = new IndexedMatrix();
shapes.Add(mshape);
ChPosMatrToBullet(pos, rot, ref mtransform);
CollisionShape mcom = shapes[0];
((CompoundShape)mcom).AddChildShape(ref mtransform, mshape);
// vector= | compound | old | old.. | new shape | ...
return;
}
}
/// Given point B, computes the distance from this triangle plane,
/// returning also the projection of point on the plane and other infos
/// \return the signed distance
public double PointTriangleDistance(ChVector B, //< point to be measured
ref double mu, //< returns U parametric coord of projection
ref double mv, //< returns V parametric coord of projection
ref bool is_into, //< returns true if projection falls on the triangle
ref ChVector Bprojected //< returns the position of the projected point
)
{
return(PointTriangleDistance(B, ref this.p1, ref this.p2, ref this.p3, ref mu, ref mv, ref is_into, ref Bprojected));
}
/// Add a triangle to this triangle mesh, by specifying the three coordinates.
/// This is disconnected - no vertex sharing is used even if it could be..
public override void addTriangle(ChVector vertex0, ChVector vertex1, ChVector vertex2)
{
int base_v = (int)m_vertices.Count;
m_vertices.Add(vertex0);
m_vertices.Add(vertex1);
m_vertices.Add(vertex2);
m_face_v_indices.Add(new ChVector(base_v, base_v + 1, base_v + 2));
}
public override ChVector Baricenter()
{
ChVector mb = new ChVector();
mb.x = (p1.x + p2.x + p3.x) / 3.0;
mb.y = (p1.y + p2.y + p3.y) / 3.0;
mb.z = (p1.z + p2.z + p3.z) / 3.0;
return(mb);
}
/// Given point B and a generic triangle, computes the distance from the triangle plane,
/// returning also the projection of point on the plane and other infos
/// \return the signed distance
public static double PointTriangleDistance(ChVector B, //< point to be measured
ref ChVector A1, //< point of triangle
ref ChVector A2, //< point of triangle
ref ChVector A3, //< point of triangle
ref double mu, //< returns U parametric coord of projection
ref double mv, //< returns V parametric coord of projection
ref bool is_into, //< returns true if projection falls on the triangle
ref ChVector Bprojected //< returns the position of the projected point
)
{
// defaults
is_into = false;
mu = mv = -1;
double mdistance = 10e22;
ChVector Dx, Dy, Dz, T1, T1p;
Dx = ChVector.Vsub(A2, A1);
Dz = ChVector.Vsub(A3, A1);
Dy = ChVector.Vcross(Dz, Dx);
double dylen = ChVector.Vlength(Dy);
if (Mathfx.Abs(dylen) < EPS_TRIDEGENERATE) // degenerate triangle
{
return(mdistance);
}
Dy = ChVector.Vmul(Dy, 1.0 / dylen);
ChMatrix33 <double> mA = new ChMatrix33 <double>(0);
ChMatrix33 <double> mAi = new ChMatrix33 <double>(0);
mA.Set_A_axis(Dx, Dy, Dz);
// invert triangle coordinate matrix -if singular matrix, was degenerate triangle-.
if (Mathfx.Abs(mA.FastInvert(mAi)) < 0.000001)
{
return(mdistance);
}
T1 = mAi.Matr_x_Vect(ChVector.Vsub(B, A1));
T1p = T1;
T1p.y = 0;
mu = T1.x;
mv = T1.z;
if (mu >= 0 && mv >= 0 && mv <= 1.0 - mu)
{
is_into = true;
mdistance = Mathfx.Abs(T1.y);
Bprojected = ChVector.Vadd(A1, mA.Matr_x_Vect(T1p));
}
return(mdistance);
}
public float[] reaction_cache; //< pointer to some persistent user cache of reactions
/// Basic default constructor.
public ChCollisionInfo()
{
modelA = null;
modelB = null;
vpA = new ChVector(0, 0, 0);
vpB = new ChVector(0, 0, 0);
vN = new ChVector(1, 0, 0);
distance = 0;
eff_radius = default_eff_radius;
reaction_cache = null;
}
public virtual double GetHeight(double x, double y)
{
double height = 0;
ChVector normal = new ChVector();
float friction = 0;
//bool hit = false;
bool hit = FindPoint(x, y, out height, ref normal, friction);
return(hit ? height : 0.0);
}
public static void ChPosMatrToBullet(ChVector pos, ChMatrix33 <double> rA, ref BulletXNA.LinearMath.IndexedMatrix mtransform)
{
IndexedBasisMatrix basisA = new IndexedBasisMatrix((float)rA.nm.matrix[0, 0], (float)rA.nm.matrix[0, 1], (float)rA.nm.matrix[0, 2], (float)rA.nm.matrix[1, 0],
(float)rA.nm.matrix[1, 1], (float)rA.nm.matrix[1, 2], (float)rA.nm.matrix[2, 0], (float)rA.nm.matrix[2, 1],
(float)rA.nm.matrix[2, 2]);
mtransform._basis = basisA;
mtransform._origin = new IndexedVector3((float)pos.x, (float)pos.y, (float)pos.z);
}
/// Collsion algorithm based on a paper of J. Shane Sui and John A. Hirshey II:
/// "A New Analytical Tire Model for Vehicle Dynamic Analysis" presented at 2001 MSC User Meeting
public static bool DiscTerrainCollisionEnvelope(
ChTerrain terrain, ///< [in] reference to terrain system
ChVector disc_center, ///< [in] global location of the disc center
ChVector disc_normal, ///< [in] disc normal, expressed in the global frame
double disc_radius, ///< [in] disc radius
ChFunction_Recorder areaDep, ///< [in] lookup table to calculate depth from intersection area
ref ChCoordsys contact, ///< [out] contact coordinate system (relative to the global frame)
ref double depth ///< [out] penetration depth (positive if contact occurred)
)
{
return(false);
}
/// Returns the axis aligned bounding box (AABB) of the collision model,
/// i.e. max-min along the x,y,z world axes. Remember that SyncPosition()
/// should be invoked before calling this.
/// MUST be implemented by child classes!
public override void GetAABB(ref ChVector bbmin, ref ChVector bbmax)
{
IndexedVector3 btmin = new IndexedVector3();
IndexedVector3 btmax = new IndexedVector3();
if (bt_collision_object.GetCollisionShape() == null)
{
;
}
bt_collision_object.GetCollisionShape().GetAabb(bt_collision_object.GetWorldTransform(), out btmin, out btmax);
bbmin.Set(btmin.X, btmin.Y, btmin.Z);
bbmax.Set(btmax.X, btmax.Y, btmax.Z);
}
/// Add a sphere shape to this model, for collision purposes
public override bool AddSphere(double radius, ChVector pos)
{
// adjust default inward 'safe' margin (always as radius)
this.SetSafeMargin(radius);
SphereShape mshape = new SphereShape((float)(radius + this.GetEnvelope()));
mshape.SetMargin((float)this.GetSuggestedFullMargin());
_injectShape(pos, new ChMatrix33 <double>(1), mshape);
return(true);
}
// return false if triangle has almost zero area
public bool IsDegenerated()
{
ChVector u = ChVector.Vsub(p2, p1);
ChVector v = ChVector.Vsub(p3, p1);
ChVector vcr = new ChVector();
vcr = ChVector.Vcross(u, v);
if (Mathfx.Abs(vcr.x) < EPS_TRIDEGENERATE && Math.Abs(vcr.y) < EPS_TRIDEGENERATE && Math.Abs(vcr.z) < EPS_TRIDEGENERATE)
{
return(true);
}
return(false);
}
// -----------------------------------------------------------------------------
// Return the complete state (expressed in the global frame) for the specified
// wheel body.
// -----------------------------------------------------------------------------
public ChSubsysDefs.WheelState GetWheelState()
{
ChSubsysDefs.WheelState state;
state.pos = GetWheelPos();
state.rot = GetSpindleRot();
state.lin_vel = GetSpindleLinVel();
state.ang_vel = GetSpindleAngVel();
ChVector ang_vel_loc = state.rot.RotateBack(state.ang_vel);
state.omega = ang_vel_loc.y;
return(state);
}
/// Perform a ray-hit test with the specified collision model.
public override bool RayHit(ChVector from,
ChVector to,
ChCollisionModel model,
ref ChRayhitResult mresult,
CollisionFilterGroups filter_group,
CollisionFilterGroups filter_mask)
{
IndexedVector3 btfrom = new IndexedVector3((float)from.x, (float)from.y, (float)from.z);
IndexedVector3 btto = new IndexedVector3((float)to.x, (float)to.y, (float)to.z);
BulletXNA.BulletCollision.AllHitsRayResultCallback rayCallback = new AllHitsRayResultCallback(btfrom, btto);
rayCallback.m_collisionFilterGroup = filter_group;
rayCallback.m_collisionFilterMask = filter_mask;
this.bt_collision_world.rayTest(btfrom, btto, rayCallback);
// Find the closest hit result on the specified model (if any)
int hit = -1;
float fraction = 1;
for (int i = 0; i < rayCallback.m_collisionObjects.Count; ++i)
{
if (rayCallback.m_collisionObjects[i].GetUserPointer() == model && rayCallback.m_hitFractions[i] < fraction)
{
hit = i;
fraction = rayCallback.m_hitFractions[i];
}
}
// Ray does not hit specified model
if (hit == -1)
{
mresult.hit = false;
return(false);
}
// Return the closest hit on the specified model
mresult.hit = true;
mresult.hitModel = (ChCollisionModel)(rayCallback.m_collisionObjects[hit].GetUserPointer());
mresult.abs_hitPoint.Set(rayCallback.m_hitPointWorld[hit].X, rayCallback.m_hitPointWorld[hit].Y,
rayCallback.m_hitPointWorld[hit].Z);
mresult.abs_hitNormal.Set(rayCallback.m_hitNormalWorld[hit].X, rayCallback.m_hitNormalWorld[hit].Y,
rayCallback.m_hitNormalWorld[hit].Z);
mresult.abs_hitNormal.Normalize();
mresult.dist_factor = fraction;
mresult.abs_hitPoint = mresult.abs_hitPoint - mresult.abs_hitNormal * mresult.hitModel.GetEnvelope();
return(true);
}
public virtual ChVector GetNormal(double x, double y)
{
double height = 0;
ChVector normal = new ChVector();
float friction = 0;
//bool hit = false;
bool hit = FindPoint(x, y, out height, ref normal, friction);
if (hit)
{
return(normal);
}
else
{
return(new ChVector());// null;
}
}
/// Add a cylinder to this model (default axis on Y direction), for collision purposes
public override bool AddCylinder(double rx,
double rz,
double hy,
ChVector pos,
ChMatrix33 <double> rot)
{
// adjust default inward margin (if object too thin)
this.SetSafeMargin(ChMaths.ChMin(this.GetSafeMargin(), 0.2 * ChMaths.ChMin(ChMaths.ChMin(rx, rz), 0.5 * hy)));
float arx = (float)(rx + this.GetEnvelope());
float arz = (float)(rz + this.GetEnvelope());
float ahy = (float)(hy + this.GetEnvelope());
CylinderShape mshape = new CylinderShape(new IndexedVector3(arx, ahy, arz));
mshape.SetMargin(this.GetSuggestedFullMargin());
_injectShape(pos, rot, mshape);
return(true);
}
/// Add a box shape to this model, for collision purposes
public override bool AddBox(
double hx, /// the halfsize on x axis
double hy, /// the halfsize on y axis
double hz, /// the halfsize on z axis
ChVector pos, /// the position of the box COG
ChMatrix33 <double> rot /// the rotation of the box - matrix must be orthogonal
)
{
// adjust default inward margin (if object too thin)
this.SetSafeMargin(ChMaths.ChMin(this.GetSafeMargin(), 0.2 * ChMaths.ChMin(ChMaths.ChMin(hx, hy), hz)));
float ahx = (float)(hx + this.GetEnvelope());
float ahy = (float)(hy + this.GetEnvelope());
float ahz = (float)(hz + this.GetEnvelope());
BoxShape mshape = new BoxShape(new IndexedVector3(ahx, ahy, ahz));
mshape.SetMargin((float)this.GetSuggestedFullMargin());
_injectShape(pos, rot, mshape);
return(true);
}
/// Calculate kinematics quantities based on the current state of the associated
/// wheel body.
public void CalculateKinematics(double time, ///< [in] current time
ChSubsysDefs.WheelState state, ///< [in] current state of associated wheel body
RigidTerrain terrain ///< [in] reference to the terrain system
)
{
// 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 = ChVector.Vcross(wheel_normal, Z_dir);
X_dir.Normalize();
ChVector Y_dir = ChVector.Vcross(Z_dir, X_dir);
// Tire reference coordinate system
// ChMatrix33<double> rot = new ChMatrix33<double>(0); // Needs nesting
rot.Set_A_axis(X_dir, Y_dir, Z_dir);
ChCoordsys tire_csys = new ChCoordsys(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 = Mathfx.Abs(V.x);
double zero_Vx = 1e-4;
m_slip_angle = (abs_Vx > zero_Vx) ? Math.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 = Math.Atan2(n.z, n.y);
}
/// Copy from other.
public ChCollisionInfo(ChCollisionInfo other, bool swap = false)
{
if (!swap)
{
modelA = other.modelA;
modelB = other.modelB;
vpA = other.vpA;
vpB = other.vpB;
vN = other.vN;
}
else
{
// copy by swapping models
modelA = other.modelB;
modelB = other.modelA;
vpA = other.vpB;
vpB = other.vpA;
vN = -other.vN;
}
distance = other.distance;
eff_radius = other.eff_radius;
reaction_cache = other.reaction_cache;
}
/// Perform a ray-hit test with the specified collision model.
public abstract bool RayHit(ChVector from,
ChVector to,
ChCollisionModel model,
ref ChRayhitResult mresult,
CollisionFilterGroups filter_group,
CollisionFilterGroups filter_mask);
