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