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