/// 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 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);
}
}
/// 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);
}
/// 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;
}
}
/// Enlarge a previous existing bounding box.
/// Usually it does not need to be overridden: base function uses GetBoundingBox()
/// If Rot is not null, the bounding box axes are considered rotated.
public virtual void InflateBoundingBox(ref double xmin,
ref double xmax,
ref double ymin,
ref double ymax,
ref double zmin,
ref double zmax,
ChMatrix33 <double> Rot)
{
}
public override void CovarianceMatrix(ChMatrix33 <double> C)
{
C.nm.matrix[0, 0] = p1.x * p1.x + p2.x * p2.x + p3.x * p3.x;
C.nm.matrix[1, 1] = p1.y * p1.y + p2.y * p2.y + p3.y * p3.y;
C.nm.matrix[2, 2] = p1.z * p1.z + p2.z * p2.z + p3.z * p3.z;
C.nm.matrix[0, 1] = p1.x * p1.y + p2.x * p2.y + p3.x * p3.y;
C.nm.matrix[0, 2] = p1.x * p1.z + p2.x * p2.z + p3.x * p3.z;
C.nm.matrix[1, 2] = p1.y * p1.z + p2.y * p2.z + p3.y * p3.z;
}
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)
{
//// TODO
}
/// Compute bounding box.
/// If a matrix Rot is not null, it should compute bounding box along
/// the rotated directions represented by that transformation matrix Rot.
/// It must be overridden by inherited classes.
public virtual void GetBoundingBox(ref double xmin,
ref double xmax,
ref double ymin,
ref double ymax,
ref double zmin,
ref double zmax,
ChMatrix33 <double> Rot)
{
xmin = xmax = ymin = ymax = zmin = zmax = 0.0;
}
/// 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 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);
}
public override void SyncPosition()
{
ChCoordsys mcsys = this.mcontactable.GetCsysForCollisionModel();
bt_collision_object.GetWorldTransform()._origin = new IndexedVector3(
(float)mcsys.pos.x, (float)mcsys.pos.y, (float)mcsys.pos.z);
ChMatrix33 <double> rA = new ChMatrix33 <double>(mcsys.rot);
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]);
bt_collision_object.GetWorldTransform()._basis = basisA;
}
/// 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);
}
/// 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 override 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)
)
{
for (int it = 0; it < trimesh.triangles.Length; it += 3)
{
ChVector point1 = new ChVector(0, 0, 0);
ChVector point2 = new ChVector(0, 0, 0);
ChVector point3 = new ChVector(0, 0, 0);
Vector3 p1 = trimesh.vertices[trimesh.triangles[it + 0]];
Vector3 p2 = trimesh.vertices[trimesh.triangles[it + 1]];
Vector3 p3 = trimesh.vertices[trimesh.triangles[it + 2]];
point1.x = p1.x;
point1.y = p1.y;
point1.z = p1.z;
point2.x = p2.x;
point2.y = p2.y;
point2.z = p2.z;
point3.x = p3.x;
point3.y = p3.y;
point3.z = p3.z;
this.AddTriangleProxy(point1,
point2,
point3,
new ChVector(), new ChVector(), new ChVector(),
false, false, false, false, false, false, sphereswept_thickness);
}
// iterate on triangles
/* for (int it = 0; it < trimesh.m_face_v_indices.Count; it++)
* {
*
* this.AddTriangleProxy(trimesh.m_vertices[(int)trimesh.m_face_v_indices[it].x],
* trimesh.m_vertices[(int)trimesh.m_face_v_indices[it].y],
* trimesh.m_vertices[(int)trimesh.m_face_v_indices[it].z],
* new ChVector(), new ChVector(), new ChVector(),
* false, false, false, false, false, false, sphereswept_thickness);
* }*/
/* TriangleMesh bulletMesh = new TriangleMesh();
* /// Store containing triangle indices for vertices
* for (int i = 0; i < mesh.triangles.Length; i+=3)
* {
* ChVector point1 = new ChVector(0, 0, 0);
* ChVector point2 = new ChVector(0, 0, 0);
* ChVector point3 = new ChVector(0, 0, 0);
* Vector3 p1 = mesh.vertices[mesh.triangles[i + 0]];
* Vector3 p2 = mesh.vertices[mesh.triangles[i + 1]];
* Vector3 p3 = mesh.vertices[mesh.triangles[i + 2]];
*
* point1.x = p1.x;
* point1.y = p1.y;
* point1.z = p1.z;
* point2.x = p2.x;
* point2.y = p2.y;
* point2.z = p2.z;
* point3.x = p3.x;
* point3.y = p3.y;
* point3.z = p3.z;
*
* // bulletMesh.m_weldingThreshold = ...
* bulletMesh.AddTriangle(ChVectToBullet(point1), ChVectToBullet(point2),
* ChVectToBullet(point3),
* true); // try to remove duplicate vertices
* }
*
* TriangleMesh bulletMesh = new TriangleMesh();
* for (int i = 0; i < trimesh.triangles.Length; i++)
* {
* // bulletMesh.m_weldingThreshold = ...
* bulletMesh.AddTriangle(ChVectToBullet(point1), ChVectToBullet(point2),
* ChVectToBullet(point3),
* false); // try to remove duplicate vertices
* }*/
// CollisionShape pShape = (BvhTriangleMeshShape)new btBvhTriangleMeshShape_handlemesh(bulletMesh);
// CollisionShape pShape = new TriangleMeshShape(bulletMesh);
// pShape.setMargin((btScalar)this.GetSafeMargin());
// ((btBvhTriangleMeshShape*)pShape).refitTree();
// btCollisionShape* pShape = new btGImpactMeshShape_handlemesh(bulletMesh);
// pShape.setMargin((btScalar) this.GetSafeMargin() );
//((btGImpactMeshShape_handlemesh*)pShape).updateBound();
// _injectShape(pos, rot, pShape);
return(true);
}
/// Transform all vertexes, by displacing and rotating (rotation via matrix, so also scaling if needed)
public override void Transform(ChVector displ, ChMatrix33 <double> rotscale)
{
}
/// 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,
ChMatrix33 <double> rot);
/// Transform all vertexes, by displacing and rotating (rotation via matrix, so also scaling if needed) public abstract void Transform(ChVector displ, ChMatrix33 <double> rotscale);
/// Compute the 3x3 covariance matrix (only the diagonal and upper part)
/// It should be overridden by inherited classes
// TODO: obsolete (unused)
public virtual void CovarianceMatrix(ChMatrix33 <double> C)
{
C.nm.matrix.Reset();
}
//// TODO
//// virtual ChVector<> Baricenter() const override;
public virtual void CovarianceMatrix(ChMatrix33 <double> C)
{
//// TODO
}
