public override void update(double mytime, bool update_assets)
{
// Inherit parent class:
base.update(mytime, update_assets);
// compute aux data for future reference (istantaneous pos speed accel)
ChFrameMoving <double> aframe1 = ChFrameMoving <double> .BitShiftRight((ChFrameMoving <double>) this.frame1, (this.Body1));
ChFrameMoving <double> aframe2 = ChFrameMoving <double> .BitShiftRight((ChFrameMoving <double>) this.frame2, (this.Body2));
ChFrameMoving <double> aframe12 = new ChFrameMoving <double>();
aframe2.TransformParentToLocal(aframe1, aframe12);
// multi-turn rotation code
double last_totrot = this.mrot;
double last_rot = (double)decimal.Remainder(Convert.ToDecimal(last_totrot), Convert.ToDecimal(ChMaths.CH_C_2PI));
double last_turns = last_totrot - last_rot;
double new_rot = (double)decimal.Remainder(Convert.ToDecimal(aframe12.GetRot().Q_to_Rotv().z), Convert.ToDecimal(ChMaths.CH_C_2PI));
this.mrot = last_turns + new_rot;
if (Math.Abs(new_rot + ChMaths.CH_C_2PI - last_rot) < Math.Abs(new_rot - last_rot))
{
this.mrot = last_turns + new_rot + ChMaths.CH_C_2PI;
}
if (Math.Abs(new_rot - ChMaths.CH_C_2PI - last_rot) < Math.Abs(new_rot - last_rot))
{
this.mrot = last_turns + new_rot - ChMaths.CH_C_2PI;
}
this.mrot_dt = aframe12.GetWvel_loc().z;
this.mrot_dtdt = aframe12.GetWacc_loc().z;
}
/// This function transforms a frame from the parent coordinate
/// system to 'this' local frame coordinate system.
public void TransformParentToLocal(
ChFrameMoving <Real> parent, //< frame to transform, given in parent coordinates
//ChFrameMoving<Real> local //< transformed frame, in local coordinates, will be stored here
ChFrameMoving <Real> local //< transformed frame, in local coordinates, will be stored here
)
{
// pos & rot
// ChFrame<Real> f = this;// new ChFrame<Real>();
TransformParentToLocal2(parent, local);
// pos_dt
local.coord_dt.pos = PointSpeedParentToLocal(parent.coord.pos, parent.coord_dt.pos);
// pos_dtdt
local.coord_dtdt.pos =
PointAccelerationParentToLocal(parent.coord.pos, parent.coord_dt.pos, parent.coord_dtdt.pos);
// rot_dt
local.coord_dt.rot = this.coord.rot.GetConjugate() % (parent.coord_dt.rot - coord_dt.rot % local.coord.rot);
// rot_dtdt
local.coord_dtdt.rot =
this.coord.rot.GetConjugate() %
(parent.coord_dtdt.rot - coord_dtdt.rot % local.coord.rot - (coord_dt.rot % local.coord_dt.rot) * 2);
}
public virtual void TransformParentToLocal2(
ChFrame <Real> parent, //< frame to transform, given in parent coordinates
ChFrameMoving <Real> local //< transformed frame, in local coordinates, will be stored here
)
{
local.SetCoord(TransformParentToLocal(parent.coord.pos), coord.rot.GetConjugate() % parent.coord.rot);
}
/// Apply a transformation (rotation and translation) represented by
/// another ChFrameMoving T in local coordinate. This is equivalent to
/// post-multiply this frame by the other frame T: this'= this * T; or this' = T >> this
public void ConcatenatePostTransformation(ChFrameMoving <Real> T)
{
ChFrameMoving <Real> res = FMNULL;// new ChFrameMoving<Real>();
this.TransformLocalToParent(T, res);
//*this = res;
}
// FUNCTIONS TO TRANSFORM THE FRAME ITSELF
/// Apply a transformation (rotation and translation) represented by
/// another ChFrameMoving T. This is equivalent to pre-multiply this frame
/// by the other frame T: this'= T * this; or this' = this >> T
public void ConcatenatePreTransformation(ChFrameMoving <Real> T)
{
ChFrameMoving <Real> res = FMNULL;// new ChFrameMoving<Real>();
T.TransformLocalToParent(this, res);
// this = res;
}
public ChFrameMoving <Real> BitShiftRight(ChFrameMoving <Real> Fb)
{
ChFrameMoving <Real> res = new ChFrameMoving <Real>();
Fb.TransformLocalToParent(this, res);
return(res);
}
public ChFrameMoving <Real> GetInverse()
{
ChFrameMoving <Real> tmp = new ChFrameMoving <Real>(this);
tmp.Invert();
return(tmp);
}
/// The transformation (also for speeds, accelerations) is
/// inverted in place.
/// That is if w=A*v, then A.Invert();v=A*w;
public override void Invert()
{
ChFrameMoving <Real> tmp;// = new ChFrameMoving<Real>();
ChFrameMoving <Real> unit = new ChFrameMoving <Real>();
tmp = this;
tmp.TransformParentToLocal(unit, this);
}
public override void update(double mytime, bool update_assets)
{
// Inherit parent class:
base.update(mytime, update_assets);
// compute aux data for future reference (istantaneous pos speed accel)
ChFrameMoving <double> aframe1 = ChFrameMoving <double> .BitShiftRight((ChFrameMoving <double>)(this.frame1), (this.Body1));
ChFrameMoving <double> aframe2 = ChFrameMoving <double> .BitShiftRight((ChFrameMoving <double>)(this.frame2), (this.Body2));
ChFrameMoving <double> aframe12 = new ChFrameMoving <double>();
aframe2.TransformParentToLocal(aframe1, aframe12);
this.mpos = aframe12.GetPos().x;
this.mpos_dt = aframe12.GetPos_dt().x;
this.mpos_dtdt = aframe12.GetPos_dtdt().x;
}
private ChFrameMoving <double> abs_frame = new ChFrameMoving <double>(); //< absolute frame position
/* public void Start()
* {
* FrameMoving = new ChFrameMoving<double>();
* Body = null;
* rest_coord = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));//ChCoordsys.CSYSNORM;
* motion_type = eChMarkerMotion.M_MOTION_FUNCTIONS;
* motion_axis = ChVector.VECT_Z;
* 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;
* 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);
*
* UpdateState();
* }*/
public ChMarker()
{
FrameMoving = new ChFrameMoving <double>();
Body = null;
rest_coord = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));//ChCoordsys.CSYSNORM;
motion_type = eChMarkerMotion.M_MOTION_FUNCTIONS;
motion_axis = ChVector.VECT_Z;
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;
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);
UpdateState();
}
// NOTE this class link is set to Start because it needs to be created after the ChBody/BodyFrame, otherwise it's not
// initialized by the time it gets to here.
public void Start()
{
ChFrame <double> frame = new ChFrameMoving <double>(Utils.ToChrono(transform.position), Utils.ToChrono(transform.rotation));
Initialize(body1.BodyFrame, body2.BodyFrame, frame);
// Get a handle to the associated function component and set the motor's function
var fun_component = this.GetComponent <ChFunction>();
if (fun_component != null)
{
SetSpeedFunction(fun_component);
}
ChSystem msystem = FindObjectOfType <ChSystem>();
msystem.AddLink(this);
}
/// Set the COG frame with respect to the auxiliary reference frame.
/// Note that this also moves the body absolute COG (the REF is fixed).
/// The position of contained ChMarker objects, if any, is not changed with respect
/// to the reference.
public void SetFrame_COG_to_REF(ChFrame <double> mloc)
{
// Problem here, old_cog_to_abs should get changed by this.
ChFrameMoving <double> old_cog_to_abs = ChFrameMoving <double> .FMNULL; // new ChFrameMoving<double>();//= this.BodyFrame;
ChFrameMoving <double> tmpref_to_abs = ChFrameMoving <double> .FMNULL; //new ChFrameMoving<double>();
this.BodyFrame.TransformLocalToParent(this.auxref_to_cog, tmpref_to_abs);
tmpref_to_abs.TransformLocalToParent(new ChFrameMoving <double>(mloc), this.BodyFrame); // Gets changed here.
// or, also, using overloaded operators for frames:
// tmpref_to_abs = auxref_to_cog >> *this;
// *this = ChFrameMoving<>(mloc) >> tmpref_to_abs;
ChFrameMoving <double> new_cog_to_abs = this.BodyFrame;
auxref_to_cog = mloc.GetInverse();
this.BodyFrame.TransformLocalToParent(this.auxref_to_cog, this.auxref_to_abs);
// or, also, using overloaded operators for frames:
// *this->auxref_to_abs = this->auxref_to_cog.GetInverse() >> this;
// Restore marker/forces positions, keeping unchanged respect to aux ref.
ChFrameMoving <double> cog_oldnew = old_cog_to_abs.BitShiftRight(new_cog_to_abs.GetInverse()); // This is not return correct due to old_cog_to_abs changing by this.BodyFrame!
for (int i = 0; i < marklist.Count; i++)
{
marklist[i].FrameMoving.ConcatenatePreTransformation(cog_oldnew);
marklist[i].update(ChTime);
}
// Forces: ?? to do...
/*
* HIER_FORCE_INIT
* while (HIER_FORCE_NOSTOP)
* {
* FORCEpointer->
* FORCEpointer->Update (mytime);
*
* HIER_FORCE_NEXT
* }
*/
}
public ChMarker(ChMarker other) : base(other)
{
FrameMoving = new ChFrameMoving <double>(other.FrameMoving);
Body = null;
motion_X = new ChFunction(other.motion_X.Clone());
motion_Y = new ChFunction(other.motion_Y.Clone());
motion_Z = new ChFunction(other.motion_Z.Clone());
motion_ang = new ChFunction(other.motion_ang.Clone());
motion_axis = other.motion_axis;
rest_coord = other.rest_coord;
motion_type = other.motion_type;
abs_frame = other.abs_frame;
last_rel_coord = other.last_rel_coord;
last_rel_coord_dt = other.last_rel_coord_dt;
last_time = other.last_time;
}
/// This function transforms a frame from 'this' local coordinate
/// system to parent frame coordinate system, and also transforms the speed
/// and acceleration of the frame.
public void TransformLocalToParent(
ChFrameMoving <Real> local, //< frame to transform, given in local frame coordinates
ChFrameMoving <Real> parent //< transformed frame, in parent coordinates, will be stored here
)
{
// pos & rot
//ChFrame<Real> f = this;
base.TransformLocalToParent(local, parent); // Problem here?
// pos_dt
parent.coord_dt.pos = PointSpeedLocalToParent(local.coord.pos, local.coord_dt.pos);
// pos_dtdt
parent.coord_dtdt.pos =
PointAccelerationLocalToParent(local.coord.pos, local.coord_dt.pos, local.coord_dtdt.pos);
// rot_dt
parent.coord_dt.rot = coord_dt.rot % local.coord.rot + this.coord.rot % local.coord_dt.rot;
// rot_dtdt
parent.coord_dtdt.rot = coord_dtdt.rot % local.coord.rot + (coord_dt.rot % local.coord_dt.rot) * 2 +
this.coord.rot % local.coord_dtdt.rot;
}
// OTHER FUNCTIONS
/// Returns true if coordsys is identical to other coordsys
public bool Equals(ChFrameMoving <Real> other)
{
return(this.coord.Equals(other.coord) && coord_dt.Equals(other.coord_dt) &&
coord_dtdt.Equals(other.coord_dtdt));
}
/// Returns true if coordsys is equal to other coordsys, within a tolerance 'tol'
public bool Equals(ChFrameMoving <Real> other, double tol)
{
return(this.coord.Equals(other.coord, tol) && coord_dt.Equals(other.coord_dt, tol) &&
coord_dtdt.Equals(other.coord_dtdt, tol));
}
/// Set the auxiliary reference frame with respect to the COG frame.
/// Note that this does not move the body absolute COG (the COG is fixed).
public void SetFrame_REF_to_COG(ChFrameMoving <double> mloc)
{
auxref_to_cog = mloc;
}
/// Copy constructor, build from another moving frame
public ChFrameMoving(ChFrameMoving <Real> other)
: base(other)
{
coord_dt = other.coord_dt;
coord_dtdt = other.coord_dtdt;
}
