/// Initialization based on passing two vectors (point + dir) on the /// two bodies, they will represent the X axes of the two frames (Y and Z will /// be built from the X vector via Gram Schmidt orthonormalization). /// Use the other ChLinkMateGeneric::Initialize() if you want to set the two frames directly. public virtual void Initialize(ChBodyFrame mbody1, //< first body to link ChBodyFrame mbody2, //< second body to link bool pos_are_relative, //< true: following pos. are relative to bodies. ChVector mpt1, //< origin of slave frame 1, for 1st body (rel. or abs., see flag above) ChVector mpt2, //< origin of master frame 2, for 2nd body (rel. or abs., see flag above) ChVector mnorm1, //< X axis of slave plane, for 1st body (rel. or abs., see flag above) ChVector mnorm2 //< X axis of master plane, for 2nd body (rel. or abs., see flag above) ) { Debug.Assert(mbody1 != mbody2); this.Body1 = mbody1; this.Body2 = mbody2; // this.SetSystem(mbody1.GetSystem()); this.mask.SetTwoBodiesVariables(Body1.Variables(), Body2.Variables()); ChVector mx = new ChVector(0, 0, 0); ChVector my = new ChVector(0, 0, 0); ChVector mz = new ChVector(0, 0, 0); ChVector mN = new ChVector(0, 0, 0); ChMatrix33 <double> mrot = new ChMatrix33 <double>(); ChFrame <double> mfr1 = new ChFrame <double>(); ChFrame <double> mfr2 = new ChFrame <double>(); if (pos_are_relative) { mN = mnorm1; mN.DirToDxDyDz(ref mx, ref my, ref mz, new ChVector(0, 1, 0)); mrot.Set_A_axis(mx, my, mz); mfr1.SetRot(mrot); mfr1.SetPos(mpt1); mN = mnorm2; mN.DirToDxDyDz(ref mx, ref my, ref mz, new ChVector(0, 1, 0)); mrot.Set_A_axis(mx, my, mz); mfr2.SetRot(mrot); mfr2.SetPos(mpt2); } else { ChVector temp = ChVector.VECT_Z; // from abs to body-rel mN = this.Body1.TransformDirectionParentToLocal(mnorm1); mN.DirToDxDyDz(ref mx, ref my, ref mz, temp); mrot.Set_A_axis(mx, my, mz); mfr1.SetRot(mrot); mfr1.SetPos(this.Body1.TransformPointParentToLocal(mpt1)); mN = this.Body2.TransformDirectionParentToLocal(mnorm2); mN.DirToDxDyDz(ref mx, ref my, ref mz, temp); mrot.Set_A_axis(mx, my, mz); mfr2.SetRot(mrot); mfr2.SetPos(this.Body2.TransformPointParentToLocal(mpt2)); } this.frame1 = mfr1; this.frame2 = mfr2; }