// -----------------------------------------------------------------------------
// 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;
}
}
/// Returns true if coordsys is equal to other coordsys, within a tolerance 'tol'
/* public bool Equals(ChCoordsys other, double tol)
* {
* return rot.Equals(other.rot, tol) && pos.Equals(other.pos, tol);
* }*/
public static ChCoordsys BitShiftRight(ChCoordsys a, ChCoordsys Fb)
{
return(Fb.TransformLocalToParent(a));
}
