public ChShaftsBody(ChShaftsBody other)
{
torque_react = other.torque_react;
shaft_dir = other.shaft_dir;
shaft = null;
body = null;
}
/// 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;
}
public ChLinkMarkers(ChLinkMarkers other) : base(other)
{
marker1 = null;
marker2 = null;
markID1 = other.markID1;
markID2 = other.markID2;
relM = other.relM; // copy vars
relM_dt = other.relM_dt;
relM_dtdt = other.relM_dtdt;
relAngle = other.relAngle;
relRotaxis = other.relRotaxis;
relAxis = other.relAxis;
relWvel = other.relWvel;
relWacc = other.relWacc;
dist = other.dist;
dist_dt = other.dist_dt;
C_force = other.C_force;
C_torque = other.C_torque;
Scr_force = other.Scr_force;
Scr_torque = other.Scr_torque;
}
/// Specialized initialization for springs, given the two bodies to be connected,
/// the positions of the two anchor endpoints of the spring (each expressed
/// in body or abs. coordinates) and the imposed rest length of the spring.
/// NOTE! As in ChLinkMarkers::Initialize(), the two markers are automatically
/// created and placed inside the two connected bodies.
public void Initialize(
ChBody mbody1, //< first body to link
ChBody mbody2, //< second body to link
bool pos_are_relative, //< true: following pos. are relative to bodies
ChVector mpos1, //< position of spring endpoint, for 1st body (rel. or abs., see flag above)
ChVector mpos2, //< position of spring endpoint, for 2nd body (rel. or abs., see flag above)
bool auto_rest_length = true, //< if true, initializes the rest-length as the distance between mpos1 and mpos2
double mrest_length = 0 //< imposed rest_length (no need to define, if auto_rest_length=true.)
)
{
// First, initialize as all constraint with markers.
// In this case, create the two markers also!.
base.Initialize(mbody1, mbody2, new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0)));
if (pos_are_relative)
{
marker1.Impose_Rel_Coord(new ChCoordsys(mpos1, new ChQuaternion(1, 0, 0, 0)));
marker2.Impose_Rel_Coord(new ChCoordsys(mpos2, new ChQuaternion(1, 0, 0, 0)));
}
else
{
marker1.Impose_Abs_Coord(new ChCoordsys(mpos1, new ChQuaternion(1, 0, 0, 0)));
marker2.Impose_Abs_Coord(new ChCoordsys(mpos2, new ChQuaternion(1, 0, 0, 0)));
}
ChVector AbsDist = marker1.GetAbsCoord().pos - marker2.GetAbsCoord().pos;
dist = AbsDist.Length();
spr_restlength = auto_rest_length ? dist : mrest_length;
}
// TRANSFORMATIONS, USING POSITION AND ROTATION QUATERNION
/// This function transforms a point from the parent coordinate
/// system to a local coordinate system, whose relative position
/// is given by the 'origin' translation and 'alignment' quaternion q.
/// Since the function is static, you do not need a ChTransform object, for example
/// use it as: mresult=ChTransform<>::TransformParentToLocal(mpar, morig, malign)
/// \return The point in local coordinate, as local=q*[(parent-origin)]*q
public static ChVector TransformParentToLocal(
ChVector parent, //< point to transform, given in parent coordinates
ChVector origin, //< origin of frame respect to parent, in parent coords,
ChQuaternion alignment //< rotation of frame respect to parent, in parent coords.
)
{
// It could be simply "return alignment.RotateBack(parent-origin);"
// but for faster execution do this:
double e0e0 = alignment.e0 * alignment.e0;
double e1e1 = alignment.e1 * alignment.e1;
double e2e2 = alignment.e2 * alignment.e2;
double e3e3 = alignment.e3 * alignment.e3;
double e0e1 = -alignment.e0 * alignment.e1;
double e0e2 = -alignment.e0 * alignment.e2;
double e0e3 = -alignment.e0 * alignment.e3;
double e1e2 = alignment.e1 * alignment.e2;
double e1e3 = alignment.e1 * alignment.e3;
double e2e3 = alignment.e2 * alignment.e3;
double dx = parent.x - origin.x;
double dy = parent.y - origin.y;
double dz = parent.z - origin.z;
return(new ChVector(((e0e0 + e1e1) * 2.0 - 1.0) * dx + ((e1e2 - e0e3) * 2.0) * dy + ((e1e3 + e0e2) * 2.0) * dz,
((e1e2 + e0e3) * 2.0) * dx + ((e0e0 + e2e2) * 2.0 - 1.0) * dy + ((e2e3 - e0e1) * 2.0) * dz,
((e1e3 - e0e2) * 2.0) * dx + ((e2e3 + e0e1) * 2.0) * dy + ((e0e0 + e3e3) * 2.0 - 1.0) * dz));
}
public ChVector(ChVector other) //: this()
{
//data = new double[3];
this.x = other.x;
this.y = other.y;
this.z = other.z;
}
public static ChVector Vnorm(ChVector va)
{
ChVector result = new ChVector(va);
result.Normalize();
return(result);
}
public ChVector Cross(ChVector other)
{
ChVector v = ChVector.VNULL;
v.Cross(this, other);
return(v);
}
//
// 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);
}
public ChMarker(string name,
ChBody body,
ChCoordsys rel_pos,
ChCoordsys rel_pos_dt,
ChCoordsys rel_pos_dtdt)
{
// SetNameString(name);
Body = body;
motion_X = new ChFunction_Const(0); // default: no motion
motion_Y = new ChFunction_Const(0);
motion_Z = new ChFunction_Const(0);
motion_ang = new ChFunction_Const(0);
motion_axis = ChVector.VECT_Z;
rest_coord = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));//ChCoordsys.CSYSNORM;
motion_type = eChMarkerMotion.M_MOTION_FUNCTIONS;
FrameMoving.SetCoord(rel_pos);
FrameMoving.SetCoord_dt(rel_pos_dt);
FrameMoving.SetCoord_dtdt(rel_pos_dtdt);
last_rel_coord = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0)); //ChCoordsys.CSYSNORM;
last_rel_coord_dt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(0, 0, 0, 0)); // ChCoordsys.CSYSNULL;
last_time = 0;
UpdateState();
}
public static ChVector operator %(ChVector a, ChVector other)
{
ChVector v = new ChVector(0, 0, 0);
v.Cross(a, other);
return(v);
}
/// Override _all_ time, jacobian etc. updating.
/// In detail, it computes jacobians, violations, etc. and stores
/// results in inner structures.
public override void update(double mytime, bool update_assets = true)
{
// Inherit time changes of parent class (ChLink), basically doing nothing :)
base.update(mytime, update_assets);
// compute jacobians
ChVector AbsDist = Body1.TransformPointLocalToParent(pos1) - Body2.TransformPointLocalToParent(pos2);
curr_dist = AbsDist.Length();
ChVector D2abs = ChVector.Vnorm(AbsDist);
ChVector D2relB = Body2.TransformDirectionParentToLocal(D2abs);
ChVector D2relA = Body1.TransformDirectionParentToLocal(D2abs);
ChVector CqAx = D2abs;
ChVector CqBx = -D2abs;
ChVector CqAr = -ChVector.Vcross(D2relA, pos1);
ChVector CqBr = ChVector.Vcross(D2relB, pos2);
Cx.Get_Cq_a().ElementN(0) = CqAx.x;
Cx.Get_Cq_a().ElementN(1) = CqAx.y;
Cx.Get_Cq_a().ElementN(2) = CqAx.z;
Cx.Get_Cq_a().ElementN(3) = CqAr.x;
Cx.Get_Cq_a().ElementN(4) = CqAr.y;
Cx.Get_Cq_a().ElementN(5) = CqAr.z;
Cx.Get_Cq_b().ElementN(0) = CqBx.x;
Cx.Get_Cq_b().ElementN(1) = CqBx.y;
Cx.Get_Cq_b().ElementN(2) = CqBx.z;
Cx.Get_Cq_b().ElementN(3) = CqBr.x;
Cx.Get_Cq_b().ElementN(4) = CqBr.y;
Cx.Get_Cq_b().ElementN(5) = CqBr.z;
//***TO DO*** C_dt? C_dtdt? (may be never used..)
}
public ChVector GetNormalized()
{
ChVector v = new ChVector(this);
v.Normalize();
return(v);
}
public void Q_to_AngAxis(ref double a_angle, ref ChVector a_axis)
{
double sin_squared = this.e1 * this.e1 + this.e2 * this.e2 + this.e3 * this.e3;
// For non-zero rotation
if (sin_squared > 0)
{
double sin_theta = Math.Sqrt(sin_squared);
a_angle = 2.0 * Math.Atan2(sin_theta, e0);
double k = 1.0 / sin_theta;
a_axis.x = this.e1 * k;
a_axis.y = this.e2 * k;
a_axis.z = this.e3 * k;
a_axis.Normalize();
}
else
{
// For almost zero rotation
a_angle = 0.0;
a_axis.x = 1; // e1 * 2.0;
a_axis.y = 0; // e2 * 2.0;
a_axis.z = 0; // e3 * 2.0;
}
// Ensure that angle is always in [-PI...PI] range
var PI = (ChMaths.CH_C_PI);
if (a_angle > PI)
{
a_angle -= 2 * PI;
}
else if (a_angle < -PI)
{
a_angle += 2 * PI;
}
}
/// Set the quaternion ddq/dtdt. Inputs: the axis of ang. acceleration 'axis' (assuming it is already
/// normalized and expressed in absolute coords), the angular acceleration 'angle_dtdt' (scalar value),
/// the rotation expressed as a quaternion 'quat' and th rotation speed 'q_dt'.
public void Qdtdt_from_AngAxis(ChQuaternion q,
ChQuaternion q_dt,
double angle_dtdt,
ChVector axis)
{
this.Qdtdt_from_Aabs2(angle_dtdt * axis, q, q_dt);
}
protected ChVector react_torque = new ChVector(0, 0, 0); //< store the torque reactions, expressed in local coordinate system of link;
public ChLink()
{
//Body1 = null;
//Body2 = null;
react_force = new ChVector(0, 0, 0);
react_torque = new ChVector(0, 0, 0);
}
public ChLinkDistance()
{
pos1 = new ChVector(0, 0, 0);
pos2 = new ChVector(0, 0, 0);
distance = 0;
curr_dist = 0;
}
/// Get angle of rotation about axis of finite rotation
public double GetRotAngle()
{
ChVector vtmp = new ChVector(0, 0, 0);
double angle = 0;
coord.rot.Q_to_AngAxis(ref angle, ref vtmp);
return(angle);
}
public ChMatrix33(ChVector vec)// : this()
{
nm = new ChMatrixNM <IntInterface.Three, IntInterface.Three>(0);
// nm.matrix = new ChMatrix();
this.nm.matrix.Set33Element(0, 0, vec.x);
this.nm.matrix.Set33Element(1, 1, vec.y);
this.nm.matrix.Set33Element(2, 2, vec.z);
}
/// Sets the rotation matrix from an gale of rotation and an axis,
/// defined in _absolute_ coords. NOTE, axis must be normalized!
public void Set_A_AngAxis(double angle, //< angle of rotation, in radians
ChVector axis) //< axis of rotation, normalized
{
ChQuaternion mr = new ChQuaternion(0, 0, 0, 0); // ChQuaternion.QNULL;
mr.Q_from_AngAxis(angle, axis);
this.Set_A_quaternion(mr);
}
public ChQuaternion(double s, ChVector v) : this()
{
// data = new double[4];
this.e0 = s;
this.e1 = v.x;
this.e2 = v.y;
this.e3 = v.z;
}
// Get the time derivative from a quaternion, a speed of rotation and an axis, defined in _abs_ coords.
public static ChQuaternion Qdt_from_AngAxis(ChQuaternion quat, double angle_dt, ChVector axis)
{
ChVector W;
W = ChVector.Vmul(axis, angle_dt);
return(Qdt_from_Wabs(W, quat));
}
public ChLink(ChLink other) : base(other)
{
Body1 = null;
Body2 = null;
react_force = other.react_force;
react_torque = other.react_torque;
}
public override void IntStateScatterReactions(int off_L, ChVectorDynamic <double> L)
{
react_force = new ChVector(0, 0, 0); // Do not update 'intuitive' react force and torque here: just set as 0.
react_torque = new ChVector(0, 0, 0); // Child classes implementations should compute them.
// if (react != null)
react.matrix.PasteClippedMatrix(L.matrix, off_L, 0, react.matrix.GetRows(), 1, 0, 0);
}
/// Multiplies this matrix by a vector, like in coordinate rotation [M]*v.
/// \return The result of the multiplication, i.e. a vector.
public ChVector Matr_x_Vect(ChVector va)
{
return(new ChVector(this.nm.matrix.Get33Element(0, 0) * va.x + this.nm.matrix.Get33Element(0, 1) * va.y +
this.nm.matrix.Get33Element(0, 2) * va.z,
this.nm.matrix.Get33Element(1, 0) * va.x + this.nm.matrix.Get33Element(1, 1) * va.y +
this.nm.matrix.Get33Element(1, 2) * va.z,
this.nm.matrix.Get33Element(2, 0) * va.x + this.nm.matrix.Get33Element(2, 1) * va.y +
this.nm.matrix.Get33Element(2, 2) * va.z));
}
// Get the quaternion first derivative from the vector of angular acceleration with a specified in _absolute_ coords.
public static ChQuaternion Qdtdt_from_Aabs(ChVector a,
ChQuaternion q,
ChQuaternion q_dt)
{
ChQuaternion ret = new ChQuaternion(1, 0, 0, 0); //QNULL;
ret.Qdtdt_from_Aabs2(a, q, q_dt);
return(ret);
}
/// Given a 3x3 rotation matrix, returns the versor of Y axis.
public ChVector Get_A_Yaxis()
{
ChVector Y = new ChVector(0, 0, 0);// ChVector.VNULL;
Y.x = this.nm.matrix.Get33Element(0, 1);
Y.y = this.nm.matrix.Get33Element(1, 1);
Y.z = this.nm.matrix.Get33Element(2, 1);
return(Y);
}
/// Given a 3x3 rotation matrix, returns the versor of Z axis.
public ChVector Get_A_Zaxis()
{
ChVector Z = new ChVector(0, 0, 0); //ChVector.VNULL;
Z.x = this.nm.matrix.Get33Element(0, 2);
Z.y = this.nm.matrix.Get33Element(1, 2);
Z.z = this.nm.matrix.Get33Element(2, 2);
return(Z);
}
public override void ConstraintsFetch_react(double factor = 1)
{
// From constraints to react vector:
react_force.x = -Cx.Get_l_i() * factor;
react_force.y = 0;
react_force.z = 0;
react_torque = new ChVector(0, 0, 0);
}
/// Given a 3x3 rotation matrix, returns the versor of X axis.
public ChVector Get_A_Xaxis()
{
ChVector X = new ChVector(0, 0, 0);// ChVector.VNULL;
X.x = this.nm.matrix.Get33Element(0, 0);
X.y = this.nm.matrix.Get33Element(1, 0);
X.z = this.nm.matrix.Get33Element(2, 0);
return(X);
}
