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