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