//
// UPDATING FUNCTIONS
//
/// Override _all_ time, jacobian etc. updating.
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);
if (this.Body1 != null && this.Body2 != null)
{
this.mask.SetTwoBodiesVariables(Body1.Variables(), Body2.Variables());
ChFrame <double> aframe = ChFrame <double> .BitShiftRight(this.frame1, (this.Body1));
ChVector p1_abs = aframe.GetPos();
ChFrame <double> aframe2 = ChFrame <double> .BitShiftRight(this.frame2, (this.Body2));
ChVector p2_abs = aframe2.GetPos();
ChFrame <double> bframe = new ChFrame <double>();
(this.Body2).TransformParentToLocal(aframe, bframe);
this.frame2.TransformParentToLocal(bframe, aframe);
// Now 'aframe' contains the position/rotation of frame 1 respect to frame 2, in frame 2 coords.
//***TODO*** check if it is faster to do aframe2.TransformParentToLocal(aframe, bframe); instead of two transforms above
ChMatrix33 <double> Jx1 = new ChMatrix33 <double>(0);
ChMatrix33 <double> Jx2 = new ChMatrix33 <double>(0);
ChMatrix33 <double> Jr1 = new ChMatrix33 <double>(0);
ChMatrix33 <double> Jr2 = new ChMatrix33 <double>(0);
ChMatrix33 <double> Jw1 = new ChMatrix33 <double>(0);
ChMatrix33 <double> Jw2 = new ChMatrix33 <double>(0);
ChMatrix33 <double> mtempM = new ChMatrix33 <double>(0);
ChMatrix33 <double> mtempQ = new ChMatrix33 <double>(0);
ChMatrix33 <double> abs_plane = new ChMatrix33 <double>(0);
abs_plane.nm.matrix.MatrMultiply(Body2.GetA().nm.matrix, frame2.GetA().nm.matrix);
Jx1.nm.matrix.CopyFromMatrixT(abs_plane.nm.matrix);
Jx2.nm.matrix.CopyFromMatrixT(abs_plane.nm.matrix);
Jx2.nm.matrix.MatrNeg();
Jw1.nm.matrix.MatrTMultiply(abs_plane.nm.matrix, Body1.GetA().nm.matrix);
Jw2.nm.matrix.MatrTMultiply(abs_plane.nm.matrix, Body2.GetA().nm.matrix);
mtempM.Set_X_matrix(frame1.GetPos());
Jr1.nm.matrix.MatrMultiply(Jw1.nm.matrix, mtempM.nm.matrix);
Jr1.nm.matrix.MatrNeg();
mtempM.Set_X_matrix(frame2.GetPos());
Jr2.nm.matrix.MatrMultiply(Jw2.nm.matrix, mtempM.nm.matrix);
ChVector p2p1_base2 = (Body2.GetA()).MatrT_x_Vect(ChVector.Vsub(p1_abs, p2_abs));
mtempM.Set_X_matrix(p2p1_base2);
mtempQ.nm.matrix.MatrTMultiply(frame2.GetA().nm.matrix, mtempM.nm.matrix);
Jr2.nm.matrix.MatrInc(mtempQ.nm.matrix);
Jw2.nm.matrix.MatrNeg();
// Premultiply by Jw1 and Jw2 by 0.5*[Fp(q_resid)]' to get residual as imaginary part of a quaternion.
// For small misalignment this effect is almost insignificant cause [Fp(q_resid)]=[I],
// but otherwise it is needed (if you want to use the stabilization term - if not, you can live without).
mtempM.Set_X_matrix((aframe.GetRot().GetVector()) * 0.5);
mtempM.nm.matrix[0, 0] = 0.5 * aframe.GetRot().e0;
mtempM.nm.matrix[1, 1] = 0.5 * aframe.GetRot().e0;
mtempM.nm.matrix[2, 2] = 0.5 * aframe.GetRot().e0;
mtempQ.nm.matrix.MatrTMultiply(mtempM.nm.matrix, Jw1.nm.matrix);
Jw1 = mtempQ;
mtempQ.nm.matrix.MatrTMultiply(mtempM.nm.matrix, Jw2.nm.matrix);
Jw2 = mtempQ;
int nc = 0;
if (c_x)
{
this.C.matrix.ElementN(nc) = aframe.GetPos().x;
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jx1.nm.matrix, 0, 0, 1, 3, 0, 0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jr1.nm.matrix, 0, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jx2.nm.matrix, 0, 0, 1, 3, 0, 0);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jr2.nm.matrix, 0, 0, 1, 3, 0, 3);
nc++;
}
if (c_y)
{
this.C.matrix.ElementN(nc) = aframe.GetPos().y;
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jx1.nm.matrix, 1, 0, 1, 3, 0, 0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jr1.nm.matrix, 1, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jx2.nm.matrix, 1, 0, 1, 3, 0, 0);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jr2.nm.matrix, 1, 0, 1, 3, 0, 3);
nc++;
}
if (c_z)
{
this.C.matrix.ElementN(nc) = aframe.GetPos().z;
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jx1.nm.matrix, 2, 0, 1, 3, 0, 0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jr1.nm.matrix, 2, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jx2.nm.matrix, 2, 0, 1, 3, 0, 0);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jr2.nm.matrix, 2, 0, 1, 3, 0, 3);
nc++;
}
if (c_rx)
{
this.C.matrix.ElementN(nc) = aframe.GetRot().e1;
this.mask.Constr_N(nc).Get_Cq_a().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_b().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 0, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 0, 0, 1, 3, 0, 3);
nc++;
}
if (c_ry)
{
this.C.matrix.ElementN(nc) = aframe.GetRot().e2;
this.mask.Constr_N(nc).Get_Cq_a().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_b().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 1, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 1, 0, 1, 3, 0, 3);
nc++;
}
if (c_rz)
{
this.C.matrix.ElementN(nc) = aframe.GetRot().e3;
this.mask.Constr_N(nc).Get_Cq_a().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_b().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 2, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 2, 0, 1, 3, 0, 3);
nc++;
}
}
}
/// Set shaft position and direction, for 2nd gear, in body2-relative reference.
/// The shaft direction is the Z axis of that frame.
public void Set_local_shaft2(ChFrame <double> mf)
{
local_shaft2 = mf;
}
public override void Start()
{
auxref_to_cog = new ChFrameMoving <double>();
auxref_to_abs = new ChFrameMoving <double>();
switch (materialType)
{
case MaterialType.NSC:
matsurface = gameObject.AddComponent <ChMaterialSurfaceNSC>();
matsurface.GetComponent <ChMaterialSurfaceNSC>().static_friction = friction;
matsurface.GetComponent <ChMaterialSurfaceNSC>().sliding_friction = friction;
matsurface.GetComponent <ChMaterialSurfaceNSC>().rolling_friction = rolling_friction;
matsurface.GetComponent <ChMaterialSurfaceNSC>().spinning_friction = spinning_friction;
matsurface.GetComponent <ChMaterialSurfaceNSC>().restitution = restitution;
matsurface.GetComponent <ChMaterialSurfaceNSC>().cohesion = cohesion;
matsurface.GetComponent <ChMaterialSurfaceNSC>().dampingf = dampingf;
matsurface.GetComponent <ChMaterialSurfaceNSC>().compliance = compliance;
matsurface.GetComponent <ChMaterialSurfaceNSC>().complianceT = complianceT;
matsurface.GetComponent <ChMaterialSurfaceNSC>().complianceRoll = complianceRoll;
matsurface.GetComponent <ChMaterialSurfaceNSC>().complianceSpin = complianceSpin;
break;
case MaterialType.SMC:
matsurface = gameObject.AddComponent <ChMaterialSurfaceSMC>();
matsurface.GetComponent <ChMaterialSurfaceSMC>().static_friction = friction;
matsurface.GetComponent <ChMaterialSurfaceSMC>().sliding_friction = friction;
matsurface.GetComponent <ChMaterialSurfaceSMC>().restitution = restitution;
// matsurface.GetComponent<ChMaterialSurfaceSMC>().rolling_friction = rolling_friction;
// matsurface.GetComponent<ChMaterialSurfaceSMC>().spinning_friction = spinning_friction;
matsurface.GetComponent <ChMaterialSurfaceSMC>().young_modulus = young_modulus;
matsurface.GetComponent <ChMaterialSurfaceSMC>().poisson_ratio = poisson_ratio;
matsurface.GetComponent <ChMaterialSurfaceSMC>().constant_adhesion = constant_adhesion;
matsurface.GetComponent <ChMaterialSurfaceSMC>().adhesionMultDMT = adhesionMultDMT;
matsurface.GetComponent <ChMaterialSurfaceSMC>().kn = kn;
matsurface.GetComponent <ChMaterialSurfaceSMC>().kt = kt;
matsurface.GetComponent <ChMaterialSurfaceSMC>().gn = gn;
matsurface.GetComponent <ChMaterialSurfaceSMC>().gt = gt;
break;
}
// Switch
switch (type)
{
case CollisionType.Cube:
var size = transform.localScale * 1f;
if (automaticMass)
{
mass = density * (size.x * size.y * size.z);
this.SetDensity((float)density);
this.SetMass(mass);
inertiaMoments.x = (float)((1.0 / 12.0) * mass * (Math.Pow(size.y, 2) + Math.Pow(size.z, 2)));
inertiaMoments.y = (float)((1.0 / 12.0) * mass * (Math.Pow(size.x, 2) + Math.Pow(size.z, 2)));
inertiaMoments.z = (float)((1.0 / 12.0) * mass * (Math.Pow(size.x, 2) + Math.Pow(size.y, 2)));
}
if (collide)
{
GetCollisionModel().ClearModel();
GetCollisionModel().AddBox(size.x * 0.473f, size.y * 0.473f, size.z * 0.473f, new ChVector(0, 0, 0), new ChMatrix33 <double>(1)); // radius x, radius z, height on y
GetCollisionModel().BuildModel();
SetCollide(true);
}
this.SetDensity((float)density);
this.SetMass(mass);
SetFrame_COG_to_REF(new ChFrame <double>(Utils.ToChrono(COM), new ChQuaternion(1, 0, 0, 0)));
SetInertiaXX(ToChrono(inertiaMoments));
SetInertiaXY(ToChrono(inertiaProducts));
SetBodyFixed(bodyfixed);
SetFrame_REF_to_abs(new ChFrame <double>(Utils.ToChrono(transform.position), Utils.ToChrono(transform.rotation)));
BodyFrame.SetPos_dt(ToChrono(linearVelocity));
BodyFrame.SetWvel_loc(ToChrono(angularVelocity));
ChSystem.system.AddBody(this);
break;
case CollisionType.Sphere:
var size2 = transform.localScale.y / 1.9;
if (automaticMass)
{
mass = density * ((4.0 / 3.0) * ChMaths.CH_C_PI * Math.Pow(size2, 3));
double inertia = (2.0 / 5.0) * mass * Math.Pow(size2, 2);
this.SetDensity((float)density);
this.SetMass(mass);
inertiaMoments.x = (float)inertia;
inertiaMoments.y = (float)inertia;
inertiaMoments.z = (float)inertia;
}
if (collide)
{
GetCollisionModel().ClearModel();
GetCollisionModel().AddSphere(size2, new ChVector(0, 0, 0)); // radius, radius, height on y
GetCollisionModel().BuildModel();
SetCollide(true);
}
this.SetDensity((float)density);
this.SetMass(mass);
SetFrame_COG_to_REF(new ChFrame <double>(Utils.ToChrono(COM), new ChQuaternion(1, 0, 0, 0)));
SetInertiaXX(ToChrono(inertiaMoments));
SetInertiaXY(ToChrono(inertiaProducts));
SetBodyFixed(bodyfixed);
SetFrame_REF_to_abs(new ChFrame <double>(Utils.ToChrono(transform.position), Utils.ToChrono(transform.rotation)));
BodyFrame.SetPos_dt(ToChrono(linearVelocity));
BodyFrame.SetWvel_loc(ToChrono(angularVelocity));
ChSystem.system.AddBody(this);
break;
case CollisionType.Cylinder:
var height = 2 * transform.localScale.y;
var radiusX = transform.localScale.x / 2.0;
var radiusZ = transform.localScale.z / 2.0;
radius = transform.localScale.x / 2.0;
if (automaticMass)
{
mass = density * (ChMaths.CH_C_PI * Math.Pow(radiusX, 2) * height);
this.SetDensity((float)density);
this.SetMass(mass);
inertiaMoments.x = (float)((1.0 / 12.0) * mass * (3 * Math.Pow(radiusX, 2) + Math.Pow(height, 2)));
inertiaMoments.y = (float)(0.5 * mass * Math.Pow(radiusX, 2));
inertiaMoments.z = (float)((1.0 / 12.0) * mass * (3 * Math.Pow(radiusX, 2) + Math.Pow(height, 2)));
}
if (collide)
{
GetCollisionModel().ClearModel();
GetCollisionModel().AddCylinder(radiusX, radiusZ, height * 0.5f, new ChVector(0, 0, 0), new ChMatrix33 <double>(1)); // radius, radius, height on y
GetCollisionModel().BuildModel();
SetCollide(true);
}
this.SetDensity((float)density);
this.SetMass(mass);
SetFrame_COG_to_REF(new ChFrame <double>(Utils.ToChrono(COM), new ChQuaternion(1, 0, 0, 0)));
SetInertiaXX(ToChrono(inertiaMoments));
SetInertiaXY(ToChrono(inertiaProducts));
SetBodyFixed(bodyfixed);
SetFrame_REF_to_abs(new ChFrame <double>(Utils.ToChrono(transform.position), Utils.ToChrono(transform.rotation)));
BodyFrame.SetPos_dt(ToChrono(linearVelocity));
BodyFrame.SetWvel_loc(ToChrono(angularVelocity));
ChSystem.system.AddBody(this);
break;
case CollisionType.Mesh:
Mesh mesh = GetComponent <MeshFilter>().sharedMesh;
double sweep_sphere_radius = 0.1;
// geometry.ChTriangleMeshConnected trimesh = new geometry.ChTriangleMeshConnected();
// trimesh.LoadWavefrontMesh(mesh);
// Create the collision model
if (collide)
{
GetCollisionModel().ClearModel();
GetCollisionModel().AddTriangleMesh(mesh, true, false, ChVector.VNULL, new ChMatrix33 <double>(1));
GetCollisionModel().BuildModel();
SetCollide(true);
}
this.SetDensity((float)density);
this.SetMass(mass);
SetInertiaXX(ToChrono(inertiaMoments));
SetInertiaXY(ToChrono(inertiaProducts));
SetBodyFixed(bodyfixed);
BodyFrame.SetPos(new ChVector(transform.position.x, transform.position.y, transform.position.z));
BodyFrame.SetRot(new ChQuaternion(transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z));
// BodyFrame.SetPos_dt(ToChrono(linearVelocity));
// BodyFrame.SetWvel_loc(ToChrono(angularVelocity));
// ChSystem msystem3 = FindObjectOfType<ChSystem>();
// msystem3.AddBody(this);
ChSystem.system.AddBody(this);
break;
case CollisionType.Terrain:
//Texture2D hMap = Resources.Load("Heightmap") as Texture2D;
Terrain terrain = GetComponent <Terrain>();
Texture2D texture2D = new Texture2D(512, 512, TextureFormat.RGB24, false);
myRenderTexture = terrain.terrainData.heightmapTexture;
RenderTexture.active = myRenderTexture;
texture2D.ReadPixels(new Rect(0, 0, myRenderTexture.width, myRenderTexture.height), 0, 0);
texture2D.Apply();
texture = texture2D;
// List<Vector3> verts = new List<Vector3>();
//List<int> tris = new List<int>();
int dimensions = 65;
//Bottom left section of the map, other sections are similar
for (int i = 0; i < dimensions; i++)
{
for (int j = 0; j < dimensions; j++)
{
//Add each new vertex in the plane
verts.Add(new Vector3(i, texture.GetPixel(i, j).grayscale * 50, j));
//Skip if a new square on the plane hasn't been formed
if (i == 0 || j == 0)
{
continue;
}
//Adds the index of the three vertices in order to make up each of the two tris
tris.Add(dimensions * i + j); //Top right
tris.Add(dimensions * i + j - 1); //Bottom right
tris.Add(dimensions * (i - 1) + j - 1); //Bottom left - First triangle
tris.Add(dimensions * (i - 1) + j - 1); //Bottom left
tris.Add(dimensions * (i - 1) + j); //Top left
tris.Add(dimensions * i + j); //Top right - Second triangle
}
}
Mesh procMesh = new Mesh();
procMesh.vertices = verts.ToArray(); //Assign verts, uvs, and tris to the mesh
// procMesh.uv = uvs;
procMesh.triangles = tris.ToArray();
procMesh.RecalculateNormals(); //Determines which way the triangles are facing
if (collide)
{
GetCollisionModel().ClearModel();
GetCollisionModel().AddTriangleMesh(procMesh, true, false, new ChVector(0, 0, 0), new ChMatrix33 <double>(1));
GetCollisionModel().BuildModel();
SetCollide(true);
}
this.SetDensity((float)density);
this.SetMass(mass);
SetInertiaXX(ToChrono(inertiaMoments));
SetInertiaXY(ToChrono(inertiaProducts));
SetBodyFixed(bodyfixed);
BodyFrame.SetPos(new ChVector(transform.position.x, transform.position.y, transform.position.z));
BodyFrame.SetRot(new ChQuaternion(transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z));
ChSystem.system.AddBody(this);
break;
}
}
// Updates motion laws, marker positions, etc.
public override void UpdateTime(double mytime)
{
// First, inherit to parent class
base.UpdateTime(mytime);
// Move markers 1 and 2 to align them as gear teeth
ChMatrix33 <double> ma1 = new ChMatrix33 <double>(0);
ChMatrix33 <double> ma2 = new ChMatrix33 <double>(0);
ChMatrix33 <double> mrotma = new ChMatrix33 <double>(0);
ChMatrix33 <double> marot_beta = new ChMatrix33 <double>(0);
ChVector mx;
ChVector my;
ChVector mz;
ChVector mr;
ChVector mmark1;
ChVector mmark2;
ChVector lastX;
ChVector vrota;
ChCoordsys newmarkpos = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));
ChFrame <double> abs_shaft1 = ChFrame <double> .FNULL; // new ChFrame<double>();
ChFrame <double> abs_shaft2 = ChFrame <double> .FNULL; //new ChFrame<double>();
((ChFrame <double>)Body1).TransformLocalToParent(local_shaft1, abs_shaft1);
((ChFrame <double>)Body2).TransformLocalToParent(local_shaft2, abs_shaft2);
ChVector vbdist = ChVector.Vsub(Get_shaft_pos1(), Get_shaft_pos2());
// ChVector Trad1 = ChVector.Vnorm(ChVector.Vcross(Get_shaft_dir1(), ChVector.Vnorm(ChVector.Vcross(Get_shaft_dir1(), vbdist))));
// ChVector Trad2 = ChVector.Vnorm(ChVector.Vcross(ChVector.Vnorm(ChVector.Vcross(Get_shaft_dir2(), vbdist)), Get_shaft_dir2()));
double dist = ChVector.Vlength(vbdist);
// compute actual rotation of the two wheels (relative to truss).
ChVector md1 = abs_shaft1.GetA().MatrT_x_Vect(-vbdist);
md1.z = 0;
md1 = ChVector.Vnorm(md1);
ChVector md2 = abs_shaft2.GetA().MatrT_x_Vect(-vbdist);
md2.z = 0;
md2 = ChVector.Vnorm(md2);
double periodic_a1 = ChMaths.ChAtan2(md1.x, md1.y);
double periodic_a2 = ChMaths.ChAtan2(md2.x, md2.y);
double old_a1 = a1;
double old_a2 = a2;
double turns_a1 = Math.Floor(old_a1 / ChMaths.CH_C_2PI);
double turns_a2 = Math.Floor(old_a2 / ChMaths.CH_C_2PI);
double a1U = turns_a1 * ChMaths.CH_C_2PI + periodic_a1 + ChMaths.CH_C_2PI;
double a1M = turns_a1 * ChMaths.CH_C_2PI + periodic_a1;
double a1L = turns_a1 * ChMaths.CH_C_2PI + periodic_a1 - ChMaths.CH_C_2PI;
a1 = a1M;
if (Math.Abs(a1U - old_a1) < Math.Abs(a1M - old_a1))
{
a1 = a1U;
}
if (Math.Abs(a1L - a1) < Math.Abs(a1M - a1))
{
a1 = a1L;
}
double a2U = turns_a2 * ChMaths.CH_C_2PI + periodic_a2 + ChMaths.CH_C_2PI;
double a2M = turns_a2 * ChMaths.CH_C_2PI + periodic_a2;
double a2L = turns_a2 * ChMaths.CH_C_2PI + periodic_a2 - ChMaths.CH_C_2PI;
a2 = a2M;
if (Math.Abs(a2U - old_a2) < Math.Abs(a2M - old_a2))
{
a2 = a2U;
}
if (Math.Abs(a2L - a2) < Math.Abs(a2M - a2))
{
a2 = a2L;
}
// compute new markers coordsystem alignment
my = ChVector.Vnorm(vbdist);
mz = Get_shaft_dir1();
mx = ChVector.Vnorm(ChVector.Vcross(my, mz));
mr = ChVector.Vnorm(ChVector.Vcross(mz, mx));
mz = ChVector.Vnorm(ChVector.Vcross(mx, my));
ChVector mz2, mx2, mr2, my2;
my2 = my;
mz2 = Get_shaft_dir2();
mx2 = ChVector.Vnorm(ChVector.Vcross(my2, mz2));
mr2 = ChVector.Vnorm(ChVector.Vcross(mz2, mx2));
ma1.Set_A_axis(mx, my, mz);
// rotate csys because of beta
vrota.x = 0.0;
vrota.y = beta;
vrota.z = 0.0;
mrotma.Set_A_Rxyz(vrota);
marot_beta.nm.matrix.MatrMultiply(ma1.nm.matrix, mrotma.nm.matrix);
// rotate csys because of alpha
vrota.x = 0.0;
vrota.y = 0.0;
vrota.z = alpha;
if (react_force.x < 0)
{
vrota.z = alpha;
}
else
{
vrota.z = -alpha;
}
mrotma.Set_A_Rxyz(vrota);
ma1.nm.matrix.MatrMultiply(marot_beta.nm.matrix, mrotma.nm.matrix);
ma2.nm.matrix.CopyFromMatrix(ma1.nm.matrix);
// is a bevel gear?
double be = Math.Acos(ChVector.Vdot(Get_shaft_dir1(), Get_shaft_dir2()));
bool is_bevel = true;
if (Math.Abs(ChVector.Vdot(Get_shaft_dir1(), Get_shaft_dir2())) > 0.96)
{
is_bevel = false;
}
// compute wheel radii so that:
// w2 = - tau * w1
if (!is_bevel)
{
double pardist = ChVector.Vdot(mr, vbdist);
double inv_tau = 1.0 / tau;
if (!epicyclic)
{
r2 = pardist - pardist / (inv_tau + 1.0);
}
else
{
r2 = pardist - (tau * pardist) / (tau - 1.0);
}
r1 = r2 * tau;
}
else
{
double gamma2;
if (!epicyclic)
{
gamma2 = be / (tau + 1.0);
}
else
{
gamma2 = be / (-tau + 1.0);
}
double al = ChMaths.CH_C_PI - Math.Acos(ChVector.Vdot(Get_shaft_dir2(), my));
double te = ChMaths.CH_C_PI - al - be;
double fd = Math.Sin(te) * (dist / Math.Sin(be));
r2 = fd * Math.Tan(gamma2);
r1 = r2 * tau;
}
// compute markers positions, supposing they
// stay on the ideal wheel contact point
mmark1 = ChVector.Vadd(Get_shaft_pos2(), ChVector.Vmul(mr2, r2));
mmark2 = mmark1;
contact_pt = mmark1;
// correct marker 1 position if phasing is not correct
if (checkphase)
{
double realtau = tau;
if (epicyclic)
{
realtau = -tau;
}
double m_delta;
m_delta = -(a2 / realtau) - a1 - phase;
if (m_delta > ChMaths.CH_C_PI)
{
m_delta -= (ChMaths.CH_C_2PI); // range -180..+180 is better than 0...360
}
if (m_delta > (ChMaths.CH_C_PI / 4.0))
{
m_delta = (ChMaths.CH_C_PI / 4.0); // phase correction only in +/- 45°
}
if (m_delta < -(ChMaths.CH_C_PI / 4.0))
{
m_delta = -(ChMaths.CH_C_PI / 4.0);
}
vrota.x = vrota.y = 0.0;
vrota.z = -m_delta;
mrotma.Set_A_Rxyz(vrota); // rotate about Z of shaft to correct
mmark1 = abs_shaft1.GetA().MatrT_x_Vect(ChVector.Vsub(mmark1, Get_shaft_pos1()));
mmark1 = mrotma.Matr_x_Vect(mmark1);
mmark1 = ChVector.Vadd(abs_shaft1.GetA().Matr_x_Vect(mmark1), Get_shaft_pos1());
}
// Move Shaft 1 along its direction if not aligned to wheel
double offset = ChVector.Vdot(Get_shaft_dir1(), (contact_pt - Get_shaft_pos1()));
ChVector moff = Get_shaft_dir1() * offset;
if (Math.Abs(offset) > 0.0001)
{
local_shaft1.SetPos(local_shaft1.GetPos() + Body1.TransformDirectionParentToLocal(moff));
}
// ! Require that the BDF routine of marker won't handle speed and acc.calculus of the moved marker 2!
marker2.SetMotionType(ChMarker.eChMarkerMotion.M_MOTION_EXTERNAL);
marker1.SetMotionType(ChMarker.eChMarkerMotion.M_MOTION_EXTERNAL);
// move marker1 in proper positions
newmarkpos.pos = mmark1;
newmarkpos.rot = ma1.Get_A_quaternion();
marker1.Impose_Abs_Coord(newmarkpos); // move marker1 into teeth position
// move marker2 in proper positions
newmarkpos.pos = mmark2;
newmarkpos.rot = ma2.Get_A_quaternion();
marker2.Impose_Abs_Coord(newmarkpos); // move marker2 into teeth position
// imposed relative positions/speeds
deltaC.pos = ChVector.VNULL;
deltaC_dt.pos = ChVector.VNULL;
deltaC_dtdt.pos = ChVector.VNULL;
deltaC.rot = ChQuaternion.QUNIT; // no relative rotations imposed!
deltaC_dt.rot = ChQuaternion.QNULL;
deltaC_dtdt.rot = ChQuaternion.QNULL;
}
/// 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;
}
