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