/// Compute the infinity norm of the quaternion, that is the maximum absolute value of one of its elements. public double LengthInf() { double e0e1 = Math.Max(Mathfx.Abs(this.e0), Mathfx.Abs(this.e1)); double e0e1e2 = Math.Max(e0e1, Mathfx.Abs(this.e2)); return(Math.Max(e0e1e2, Mathfx.Abs(this.e3))); }
// compute triangle normal public bool Normal(ref ChVector N) { ChVector u; u = ChVector.Vsub(p2, p1); ChVector v; v = ChVector.Vsub(p3, p1); ChVector n; n = ChVector.Vcross(u, v); double len = ChVector.Vlength(n); if (Mathfx.Abs(len) > EPS_TRIDEGENERATE) { N = ChVector.Vmul(n, (1.0 / len)); } else { return(false); } return(true); }
/// Given point B and a generic triangle, computes the distance from the triangle plane, /// returning also the projection of point on the plane and other infos /// \return the signed distance public static double PointTriangleDistance(ChVector B, //< point to be measured ref ChVector A1, //< point of triangle ref ChVector A2, //< point of triangle ref ChVector A3, //< point of triangle ref double mu, //< returns U parametric coord of projection ref double mv, //< returns V parametric coord of projection ref bool is_into, //< returns true if projection falls on the triangle ref ChVector Bprojected //< returns the position of the projected point ) { // defaults is_into = false; mu = mv = -1; double mdistance = 10e22; ChVector Dx, Dy, Dz, T1, T1p; Dx = ChVector.Vsub(A2, A1); Dz = ChVector.Vsub(A3, A1); Dy = ChVector.Vcross(Dz, Dx); double dylen = ChVector.Vlength(Dy); if (Mathfx.Abs(dylen) < EPS_TRIDEGENERATE) // degenerate triangle { return(mdistance); } Dy = ChVector.Vmul(Dy, 1.0 / dylen); ChMatrix33 <double> mA = new ChMatrix33 <double>(0); ChMatrix33 <double> mAi = new ChMatrix33 <double>(0); mA.Set_A_axis(Dx, Dy, Dz); // invert triangle coordinate matrix -if singular matrix, was degenerate triangle-. if (Mathfx.Abs(mA.FastInvert(mAi)) < 0.000001) { return(mdistance); } T1 = mAi.Matr_x_Vect(ChVector.Vsub(B, A1)); T1p = T1; T1p.y = 0; mu = T1.x; mv = T1.z; if (mu >= 0 && mv >= 0 && mv <= 1.0 - mu) { is_into = true; mdistance = Mathfx.Abs(T1.y); Bprojected = ChVector.Vadd(A1, mA.Matr_x_Vect(T1p)); } return(mdistance); }
/// Gets the norm infinite of the matrix, i.e. the max. /// of its elements in absolute value. public double NormInf() { double norm = 0; for (int nel = 0; nel < rows * columns; ++nel) { if ((Mathfx.Abs(ElementN(nel))) > norm) { norm = Mathfx.Abs(ElementN(nel)); } } return(norm); }
// return false if triangle has almost zero area public bool IsDegenerated() { ChVector u = ChVector.Vsub(p2, p1); ChVector v = ChVector.Vsub(p3, p1); ChVector vcr = new ChVector(); vcr = ChVector.Vcross(u, v); if (Mathfx.Abs(vcr.x) < EPS_TRIDEGENERATE && Math.Abs(vcr.y) < EPS_TRIDEGENERATE && Math.Abs(vcr.z) < EPS_TRIDEGENERATE) { return(true); } return(false); }
/// Returns true if vector equals another vector, within a tolerance 'tol' public bool Equals(ChMatrix other, double tol) { if ((other.GetColumns() != this.columns) || (other.GetRows() != this.rows)) { return(false); } for (int nel = 0; nel < rows * columns; ++nel) { if (Mathfx.Abs(ElementN(nel) - other.ElementN(nel)) > tol) { return(false); } } return(true); }
public int GetMaxComponent() { int idx = 0; double max = Mathfx.Abs(this.x); if (Mathfx.Abs(this.y) > max) { idx = 1; max = this.y; } if (Mathfx.Abs(this.z) > max) { idx = 2; max = this.z; } return(idx); }
public void DirToDxDyDz(ref ChVector Vx, ref ChVector Vy, ref ChVector Vz, ChVector Vsingular) { // set Vx. if (this.IsNull()) { Vx = new ChVector(1, 0, 0); } else { Vx = this.GetNormalized(); } Vz.Cross(Vx, Vsingular); double zlen = Vz.Length(); // if near singularity, change the singularity reference vector. if (zlen < 0.0001) { ChVector mVsingular = new ChVector(0, 0, 0); if (Mathfx.Abs(Vsingular.x) < 0.9) { mVsingular = new ChVector(1, 0, 0); } else if (Mathfx.Abs(Vsingular.y) < 0.9) { mVsingular = new ChVector(0, 1, 0); } else if (Mathfx.Abs(Vsingular.z) < 0.9) { mVsingular = new ChVector(0, 0, 1); } Vz.Cross(Vx, mVsingular); zlen = Vz.Length(); // now should be nonzero length. } // normalize Vz. Vz.Scale(1 / zlen); // compute Vy. Vy.Cross(Vz, Vx); }
// From non-normalized x direction, to versors DxDyDz. // Vsingular sets the normal to the plane on which Dz must lie. public static void XdirToDxDyDz(ChVector Vxdir, ChVector Vsingular, ref ChVector Vx, ref ChVector Vy, ref ChVector Vz) { ChVector mVnull = new ChVector(0, 0, 0); double zlen; if (Vequal(Vxdir, mVnull)) { Vx = new ChVector(1, 0, 0); } else { Vx = Vnorm(Vxdir); } Vz = Vcross(Vx, Vsingular); zlen = Vlength(Vz); // If close to singularity, change reference vector if (zlen < 0.0001) { ChVector mVsingular = new ChVector(0, 0, 0); if (Mathfx.Abs(Vsingular.x) < 0.9) { mVsingular = new ChVector(0, 0, 1); } if (Mathfx.Abs(Vsingular.y) < 0.9) { mVsingular = new ChVector(0, 1, 0); } if (Mathfx.Abs(Vsingular.z) < 0.9) { mVsingular = new ChVector(1, 0, 0); } Vz = Vcross(Vx, mVsingular); zlen = Vlength(Vz); // now should be nonzero length. } // normalize Vz Vz = Vmul(Vz, 1.0 / zlen); // compute Vy Vy = Vcross(Vz, Vx); }
public static void Q_to_AngAxis(ChQuaternion quat, ref double angle, ref ChVector axis) { if (Mathfx.Abs(quat.e0) < 0.99999999) { double arg = Math.Acos(quat.e0); double invsine = 1 / Math.Sin(arg); ChVector vtemp = ChVector.VNULL; //new ChVector(0, 0, 0); vtemp.x = invsine * quat.e1; vtemp.y = invsine * quat.e2; vtemp.z = invsine * quat.e3; angle = 2 * arg; axis = ChVector.Vnorm(vtemp); } else { axis.x = 1; axis.y = 0; axis.z = 0; angle = 0; } }
/// Calculate kinematics quantities based on the current state of the associated /// wheel body. public void CalculateKinematics(double time, ///< [in] current time ChSubsysDefs.WheelState state, ///< [in] current state of associated wheel body RigidTerrain terrain ///< [in] reference to the terrain system ) { // Wheel normal (expressed in global frame) ChVector wheel_normal = state.rot.GetYaxis(); // Terrain normal at wheel location (expressed in global frame) ChVector Z_dir = terrain.GetNormal(state.pos.x, state.pos.y); // Longitudinal (heading) and lateral directions, in the terrain plane ChVector X_dir = ChVector.Vcross(wheel_normal, Z_dir); X_dir.Normalize(); ChVector Y_dir = ChVector.Vcross(Z_dir, X_dir); // Tire reference coordinate system // ChMatrix33<double> rot = new ChMatrix33<double>(0); // Needs nesting rot.Set_A_axis(X_dir, Y_dir, Z_dir); ChCoordsys tire_csys = new ChCoordsys(state.pos, rot.Get_A_quaternion()); // Express wheel linear velocity in tire frame ChVector V = tire_csys.TransformDirectionParentToLocal(state.lin_vel); // Express wheel normal in tire frame ChVector n = tire_csys.TransformDirectionParentToLocal(wheel_normal); // Slip angle double abs_Vx = Mathfx.Abs(V.x); double zero_Vx = 1e-4; m_slip_angle = (abs_Vx > zero_Vx) ? Math.Atan(V.y / abs_Vx) : 0; // Longitudinal slip m_longitudinal_slip = (abs_Vx > zero_Vx) ? -(V.x - state.omega * GetRadius()) / abs_Vx : 0; // Camber angle m_camber_angle = Math.Atan2(n.z, n.y); }
// ANGLE CONVERSIONS /// Computes the atan2, returning angle given cosine and sine. public static double ChAtan2(double mcos, double msin) { double ret; if (Mathfx.Abs(mcos) < 0.707) { ret = Math.Cos(mcos); if (msin < 0.0) { ret = -ret; } } else { ret = Math.Sin(msin); if (mcos < 0.0) { ret = CH_C_PI - ret; } } return(ret); }
public bool Equals(ChQuaternion other, double tol) { // dynamic a = other; return((Mathfx.Abs(other.e0 - this.e0) < tol) && (Mathfx.Abs(other.e1 - this.e1) < tol) && (Mathfx.Abs(other.e2 - this.e2) < tol) && (Mathfx.Abs(other.e3 - this.e3) < tol)); }
//public System.Object lockObject; public void AddSpan(uint bottom, uint top, int area, int voxelWalkableClimb) { VoxelSpan span = new VoxelSpan(bottom, top, area); //lock (lockObject) { if (firstSpan == null) { firstSpan = span; return; } VoxelSpan prev = null; VoxelSpan cSpan = firstSpan; //for (VoxelSpan cSpan = firstSpan; cSpan != null; cSpan = cSpan.next) { while (cSpan != null) { if (cSpan.bottom > span.top) { break; } else if (cSpan.top < span.bottom) { prev = cSpan; cSpan = cSpan.next; } else { if (cSpan.bottom < bottom) { span.bottom = cSpan.bottom; } if (cSpan.top > top) { span.top = cSpan.top; } //1 is flagMergeDistance, when a walkable flag is favored before an unwalkable one if (Mathfx.Abs((int)span.top - (int)cSpan.top) <= voxelWalkableClimb) { span.area = Mathfx.Max(span.area, cSpan.area); } VoxelSpan next = cSpan.next; if (prev != null) { prev.next = next; } else { firstSpan = next; } cSpan = next; /*cSpan.bottom = cSpan.bottom < bottom ? cSpan.bottom : bottom; * cSpan.top = cSpan.top > top ? cSpan.top : top; * * if (cSpan.bottom < span.bottom) { * span.bottom = cSpan.bottom; * } * if (cSpan.top > span.top) { * span.top = cSpan.top; * } * * span.area = Mathfx.Min (span.area,cSpan.area); * VoxelSpan next = cSpan.next; * * if (prev != null) { * prev.next = next; * } else { * firstSpan = next; * } * cSpan = next;*/ } } if (prev != null) { span.next = prev.next; prev.next = span; } else { span.next = firstSpan; firstSpan = span; } //} }
/// Compute the infinity norm of the vector, that is the maximum absolute value of one of its elements. public double LengthInf() { return(Math.Max(Math.Max(Mathfx.Abs(this.x), Mathfx.Abs(this.y)), Mathfx.Abs(this.z))); }
/// Performs the solution of the problem. /// \return the maximum constraint violation after termination. public override double Solve(ref ChSystemDescriptor sysd //< system description with constraints and variables ) { List <ChConstraint> mconstraints = sysd.GetConstraintsList(); List <ChVariables> mvariables = sysd.GetVariablesList(); tot_iterations = 0; double maxviolation = 0.0; double maxdeltalambda = 0.0; int i_friction_comp = 0; double[] old_lambda_friction = new double[3]; // 1) Update auxiliary data in all constraints before starting, // that is: g_i=[Cq_i]*[invM_i]*[Cq_i]' and [Eq_i]=[invM_i]*[Cq_i]' for (int ic = 0; ic < mconstraints.Count; ic++) { mconstraints[ic].Update_auxiliary(); } // Average all g_i for the triplet of contact constraints n,u,v. // int j_friction_comp = 0; double[] gi_values = new double[3]; for (int ic = 0; ic < mconstraints.Count; ic++) { if (mconstraints[ic].GetMode() == eChConstraintMode.CONSTRAINT_FRIC) { gi_values[j_friction_comp] = mconstraints[ic].Get_g_i(); j_friction_comp++; if (j_friction_comp == 3) { double average_g_i = (gi_values[0] + gi_values[1] + gi_values[2]) / 3.0; mconstraints[ic - 2].Set_g_i(average_g_i); mconstraints[ic - 1].Set_g_i(average_g_i); mconstraints[ic - 0].Set_g_i(average_g_i); j_friction_comp = 0; } } } // 2) Compute, for all items with variables, the initial guess for // still unconstrained system: for (int iv = 0; iv < mvariables.Count; iv++) { if (mvariables[iv].IsActive()) { mvariables[iv].Compute_invMb_v(mvariables[iv].Get_qb().matrix, mvariables[iv].Get_fb().matrix); // q = [M]'*fb } } // 3) For all items with variables, add the effect of initial (guessed) // lagrangian reactions of constraints, if a warm start is desired. // Otherwise, if no warm start, simply resets initial lagrangians to zero. if (warm_start) { for (int ic = 0; ic < mconstraints.Count; ic++) { if (mconstraints[ic].IsActive()) { mconstraints[ic].Increment_q(mconstraints[ic].Get_l_i()); } } } else { for (int ic = 0; ic < mconstraints.Count; ic++) { mconstraints[ic].Set_l_i(0.0); } } // 4) Perform the iteration loops // for (int iter = 0; iter < max_iterations; iter++) { // The iteration on all constraints // maxviolation = 0; maxdeltalambda = 0; i_friction_comp = 0; for (int ic = 0; ic < mconstraints.Count; ic++) { // skip computations if constraint not active. if (mconstraints[ic].IsActive()) { // compute residual c_i = [Cq_i]*q + b_i + cfm_i*l_i double mresidual = mconstraints[ic].Compute_Cq_q() + mconstraints[ic].Get_b_i() + mconstraints[ic].Get_cfm_i() * mconstraints[ic].Get_l_i(); // true constraint violation may be different from 'mresidual' (ex:clamped if unilateral) double candidate_violation = Mathfx.Abs(mconstraints[ic].Violation(mresidual)); // compute: delta_lambda = -(omega/g_i) * ([Cq_i]*q + b_i + cfm_i*l_i ) double deltal = (omega / mconstraints[ic].Get_g_i()) * (-mresidual); if (mconstraints[ic].GetMode() == eChConstraintMode.CONSTRAINT_FRIC) { candidate_violation = 0; // update: lambda += delta_lambda; old_lambda_friction[i_friction_comp] = mconstraints[ic].Get_l_i(); mconstraints[ic].Set_l_i(old_lambda_friction[i_friction_comp] + deltal); i_friction_comp++; if (i_friction_comp == 1) { candidate_violation = Mathfx.Abs(ChMaths.ChMin(0.0, mresidual)); } if (i_friction_comp == 3) { mconstraints[ic - 2].Project(); // the N normal component will take care of N,U,V double new_lambda_0 = mconstraints[ic - 2].Get_l_i(); double new_lambda_1 = mconstraints[ic - 1].Get_l_i(); double new_lambda_2 = mconstraints[ic - 0].Get_l_i(); // Apply the smoothing: lambda= sharpness*lambda_new_projected + (1-sharpness)*lambda_old if (this.shlambda != 1.0) { new_lambda_0 = shlambda * new_lambda_0 + (1.0 - shlambda) * old_lambda_friction[0]; new_lambda_1 = shlambda * new_lambda_1 + (1.0 - shlambda) * old_lambda_friction[1]; new_lambda_2 = shlambda * new_lambda_2 + (1.0 - shlambda) * old_lambda_friction[2]; mconstraints[ic - 2].Set_l_i(new_lambda_0); mconstraints[ic - 1].Set_l_i(new_lambda_1); mconstraints[ic - 0].Set_l_i(new_lambda_2); } double true_delta_0 = new_lambda_0 - old_lambda_friction[0]; double true_delta_1 = new_lambda_1 - old_lambda_friction[1]; double true_delta_2 = new_lambda_2 - old_lambda_friction[2]; mconstraints[ic - 2].Increment_q(true_delta_0); mconstraints[ic - 1].Increment_q(true_delta_1); mconstraints[ic - 0].Increment_q(true_delta_2); if (this.record_violation_history) { maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Mathfx.Abs(true_delta_0)); maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Mathfx.Abs(true_delta_1)); maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Mathfx.Abs(true_delta_2)); } i_friction_comp = 0; } } else { // update: lambda += delta_lambda; double old_lambda = mconstraints[ic].Get_l_i(); mconstraints[ic].Set_l_i(old_lambda + deltal); // If new lagrangian multiplier does not satisfy inequalities, project // it into an admissible orthant (or, in general, onto an admissible set) mconstraints[ic].Project(); // After projection, the lambda may have changed a bit.. double new_lambda = mconstraints[ic].Get_l_i(); // Apply the smoothing: lambda= sharpness*lambda_new_projected + (1-sharpness)*lambda_old if (this.shlambda != 1.0) { new_lambda = shlambda * new_lambda + (1.0 - shlambda) * old_lambda; mconstraints[ic].Set_l_i(new_lambda); } double true_delta = new_lambda - old_lambda; // For all items with variables, add the effect of incremented // (and projected) lagrangian reactions: mconstraints[ic].Increment_q(true_delta); if (this.record_violation_history) { maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Mathfx.Abs(true_delta)); } } maxviolation = ChMaths.ChMax(maxviolation, Mathfx.Abs(candidate_violation)); } // end IsActive() } // end loop on constraints // For recording into violation history, if debugging if (this.record_violation_history) { AtIterationEnd(maxviolation, maxdeltalambda, iter); } tot_iterations++; // Terminate the loop if violation in constraints has been successfully limited. if (maxviolation < tolerance) { break; } } // end iteration loop*/ return(maxviolation); }
public static DbvtNode TopDown(Dbvt pdbvt, ObjectArray <DbvtNode> leaves, int bu_treshold) { if (leaves.Count > 1) { if (leaves.Count > bu_treshold) { DbvtAabbMm vol = Bounds(leaves); IndexedVector3 org = vol.Center(); ObjectArray <DbvtNode>[] sets = { new ObjectArray <DbvtNode>(), new ObjectArray <DbvtNode>() }; int bestaxis = -1; int bestmidp = leaves.Count; int[] a1 = new int[] { 0, 0 }; int[] a2 = new int[] { 0, 0 }; int[] a3 = new int[] { 0, 0 }; int[][] splitcount = new int[][] { a1, a2, a3 }; int i; for (i = 0; i < leaves.Count; ++i) { IndexedVector3 x = leaves[i].volume.Center() - org; for (int j = 0; j < 3; ++j) { ++splitcount[j][IndexedVector3.Dot(x, axis[j]) > 0 ? 1 : 0]; } } for (i = 0; i < 3; ++i) { if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) { int midp = (int)Mathfx.Abs((splitcount[i][0] - splitcount[i][1])); if (midp < bestmidp) { bestaxis = i; bestmidp = midp; } } } if (bestaxis >= 0) { sets[0].EnsureCapacity(splitcount[bestaxis][0]); sets[1].EnsureCapacity(splitcount[bestaxis][1]); Split(leaves, sets[0], sets[1], ref org, ref axis[bestaxis]); } else { sets[0].EnsureCapacity(leaves.Count / 2 + 1); sets[1].EnsureCapacity(leaves.Count / 2); for (int i2 = 0, ni = leaves.Count; i2 < ni; ++i2) { sets[i2 & 1].Add(leaves[i2]); } } DbvtNode node = CreateNode(pdbvt, null, ref vol, null); node._children[0] = TopDown(pdbvt, sets[0], bu_treshold); node._children[1] = TopDown(pdbvt, sets[1], bu_treshold); node._children[0].parent = node; node._children[1].parent = node; return(node); } else { BottomUp(pdbvt, leaves); return(leaves[0]); } } return(leaves[0]); }
/// Return true if this vector is equal to another vector, within a tolerance 'tol'. public bool Equals(ChVector other, double tol) { return((Mathfx.Abs(other.x - this.x) < tol) && (Mathfx.Abs(other.y - this.y) < tol) && (Mathfx.Abs(other.z - this.z) < tol)); }