Here you can find the source of mod2pi(double vin)
static public double mod2pi(double vin)
//package com.java2s; public class Main { static double twopi_inv = 0.5 / Math.PI; static double twopi = 2.0 * Math.PI; /** Ensure that v is [-PI, PI] **/ static public double mod2pi(double vin) { double v; if (vin < 0) v = -mod2pi_pos(-vin);//ww w .j a v a2 s . c om else v = mod2pi_pos(vin); // Validation test: // if (v < -Math.PI || v > Math.PI) // System.out.printf("%10.3f -> %10.3f\n", vin, v); return v; } /** Returns a value of v wrapped such that ref and v differ by no * more +/-PI **/ static public double mod2pi(double ref, double v) { return ref + mod2pi(v - ref); } static private double mod2pi_pos(double vin) { double q = vin * twopi_inv + 0.5; int qi = (int) q; return vin - qi * twopi; } }