//
// FUNCTIONS
//
/// Initialize again this constraint.
public virtual void Reset(Ta mobjA, //< ChContactable object A
Tb mobjB, //< ChContactable object B
collision.ChCollisionInfo cinfo //< data for the contact pair
)
{
//Debug.Assert(mobjA);
//Debug.Assert(mobjB);
this.objA = mobjA;
this.objB = mobjB;
this.p1 = cinfo.vpA;
this.p2 = cinfo.vpB;
this.normal = cinfo.vN;
this.norm_dist = cinfo.distance;
this.eff_radius = cinfo.eff_radius;
// Contact plane
ChVector Vx = new ChVector(0, 0, 0);
ChVector Vy = new ChVector(0, 0, 0);
ChVector Vz = new ChVector(0, 0, 0);
ChVector.XdirToDxDyDz(normal, ChVector.VECT_Y, ref Vx, ref Vy, ref Vz);
contact_plane.Set_A_axis(Vx, Vy, Vz);
}
/// Get the link coordinate system, expressed relative to Body2 (the 'master'
/// body). This represents the 'main' reference of the link: reaction forces
/// are expressed in this coordinate system.
/// (It is the coordinate system of the contact plane relative to Body2)
public override ChCoordsys GetLinkRelativeCoords()
{
//ChVector D2local;
ChVector D2temp = (ChVector.Vnorm(Body1.TransformPointLocalToParent(pos1) - Body2.TransformPointLocalToParent(pos2)));
ChVector D2rel = Body2.TransformDirectionParentToLocal(D2temp);
ChVector Vx = new ChVector(0, 0, 0), Vy = new ChVector(0, 0, 0), Vz = new ChVector(0, 0, 0);
//ChMatrix33<double> rel_matrix = new ChMatrix33<double>();
ChVector.XdirToDxDyDz(D2rel, ChVector.VECT_Y, ref Vx, ref Vy, ref Vz);
rel_matrix.Set_A_axis(Vx, Vy, Vz);
ChQuaternion Ql2 = rel_matrix.Get_A_quaternion();
return(new ChCoordsys(pos2, Ql2));
}
// -----------------------------------------------------------------------------
// Draw a spring in 3D space, with given color.
// -----------------------------------------------------------------------------
public static void drawSpring(double radius,
ChVector start,
ChVector end,
int mresolution,
double turns)
{
ChMatrix33 <double> rel_matrix = new ChMatrix33 <double>(0);
ChVector dist = end - start;
ChVector Vx = new ChVector(0, 0, 0);
ChVector Vy = new ChVector(0, 0, 0);
ChVector Vz = new ChVector(0, 0, 0);
double length = dist.Length();
ChVector dir = ChVector.Vnorm(dist);
ChVector.XdirToDxDyDz(dir, ChVector.VECT_Y, ref Vx, ref Vy, ref Vz);
rel_matrix.Set_A_axis(Vx, Vy, Vz);
ChQuaternion Q12 = rel_matrix.Get_A_quaternion();
ChCoordsys mpos = new ChCoordsys(start, Q12);
double phaseA = 0;
double phaseB = 0;
double heightA = 0;
double heightB = 0;
for (int iu = 1; iu <= mresolution; iu++)
{
phaseB = turns * ChMaths.CH_C_2PI * (double)iu / (double)mresolution;
heightB = length * ((double)iu / mresolution);
ChVector V1 = new ChVector(heightA, radius * Math.Cos(phaseA), radius * Math.Sin(phaseA));
ChVector V2 = new ChVector(heightB, radius * Math.Cos(phaseB), radius * Math.Sin(phaseB));
Gizmos.color = new Color(255, 255, 0);
Gizmos.DrawLine(new Vector3((float)mpos.TransformLocalToParent(V1).x, (float)mpos.TransformLocalToParent(V1).y, (float)mpos.TransformLocalToParent(V1).z),
new Vector3((float)mpos.TransformLocalToParent(V2).x, (float)mpos.TransformLocalToParent(V2).y, (float)mpos.TransformLocalToParent(V2).z));
phaseA = phaseB;
heightA = heightB;
}
}
