public static ChVector Vnorm(ChVector va)
{
ChVector result = new ChVector(va);
result.Normalize();
return(result);
}
public void Q_to_AngAxis(ref double a_angle, ref ChVector a_axis)
{
double sin_squared = this.e1 * this.e1 + this.e2 * this.e2 + this.e3 * this.e3;
// For non-zero rotation
if (sin_squared > 0)
{
double sin_theta = Math.Sqrt(sin_squared);
a_angle = 2.0 * Math.Atan2(sin_theta, e0);
double k = 1.0 / sin_theta;
a_axis.x = this.e1 * k;
a_axis.y = this.e2 * k;
a_axis.z = this.e3 * k;
a_axis.Normalize();
}
else
{
// For almost zero rotation
a_angle = 0.0;
a_axis.x = 1; // e1 * 2.0;
a_axis.y = 0; // e2 * 2.0;
a_axis.z = 0; // e3 * 2.0;
}
// Ensure that angle is always in [-PI...PI] range
var PI = (ChMaths.CH_C_PI);
if (a_angle > PI)
{
a_angle -= 2 * PI;
}
else if (a_angle < -PI)
{
a_angle += 2 * PI;
}
}
public ChVector GetNormalized()
{
ChVector v = new ChVector(this);
v.Normalize();
return(v);
}
