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