interploate spherical coordinates

Runner.java

import java.util.ArrayList;

import javax.vecmath.Point3d;
import javax.vecmath.Point3d_karthesian;
import javax.vecmath.Point3d_spherical;
import javax.vecmath.Vector3d;

public class Runner {

	/**
	 * @param args
	 */
	public static void main(String[] args) {
		new Runner();
	}
	
	public Runner() {
		ArrayList<Point3d> points = new ArrayList<Point3d>();
		
		Point3d p1	= new Point3d(10, 0, -5);
		Point3d p2	= new Point3d(0, 10, 5);
		Point3d c	= new Point3d(0, 0, 0);
		
		points = interpolate(p1, p2, c, 1000);
		
		DebugVisualizer3D	visual					= new DebugVisualizer3D();
		int					pointSize				= 5;
		boolean				aliased					= true;
		boolean				showCoordinateSystem	= true;
		boolean				showSimulation			= false;
		//visual.renderPointCloud(points, pointSize, aliased, showCoordinateSystem, showSimulation);
		visual.renderLinePath(points, pointSize, aliased, showCoordinateSystem, showSimulation);
	}
	
	protected ArrayList<Point3d> interpolate(Point3d p1, Point3d p2, Point3d c, int num_points) {
		ArrayList<Point3d> points = new ArrayList<Point3d>();
		Point3d_karthesian	tmp_karthesian;
		Point3d_spherical	tmp_spherical;
		double curr_radius;
		double curr_phi;
		double curr_theta;
		
		Point3d_karthesian _p1	= new Point3d_karthesian(p1);
		Point3d_karthesian _p2	= new Point3d_karthesian(p2);
		Point3d_karthesian _c	= new Point3d_karthesian(c);
		
		//move xy plane up, untill center is at 0,0,0
		Vector3d offset = new Vector3d(_c);
		_p1.sub(offset);
		_p2.sub(offset);
		_c.sub(offset);
		
		Point3d_spherical sp1 = _p1.toSpherePoint();
		Point3d_spherical sp2 = _p2.toSpherePoint();
		
		System.out.println("------------------------------");
		System.out.printf("Substraction vector: %s\n", offset);
		System.out.printf("Point 1            : %s\n", _p1);
		System.out.printf("Point 2            : %s\n", _p2);
		System.out.printf("Center             : %s\n", _c);
		
		System.out.printf("Sphere Point 1     : %s\n", sp1);
		System.out.printf("Sphere Point 2     : %s\n", sp2);
		System.out.println("------------------------------");
		
		double radius1		= sp1.getRadius();
		double radius2		= sp2.getRadius();
		double radius_diff	= radius2-radius1;
		
		double phi1			= sp1.getPhi();
		double phi2			= sp2.getPhi();
		double phi_diff		= phi2-phi1;
		
		double theta1		= sp1.getTheta();
		double theta2		= sp2.getTheta();
		double theta_diff	= theta2-theta1;
		
		final double radius_step	= radius_diff	/ num_points;
		final double phi_step		= phi_diff		/ num_points;
		final double theta_step	= theta_diff	/ num_points;
		System.out.printf("Radius increment : %s\n", radius_step);
		System.out.printf("Phi    increment : %s\n", phi_step);
		System.out.printf("Theta  increment : %s\n", theta_step);
		System.out.println("------------------------------");
		
		//interpolate
		points.add(p1);
		for(int i = 1; i <= num_points; i++) {
			curr_radius	= radius1	+ radius_step	* i;
			curr_phi	= phi1		+ phi_step		* i;
			curr_theta	= theta1	+ theta_step	* i;
			
			tmp_spherical	= new Point3d_spherical(curr_radius, curr_phi, curr_theta);
			//System.out.printf("Sphere Point     : %s\n", tmp_spherical);
			
			tmp_karthesian	= tmp_spherical.toPoint3d();
			//System.out.printf("Karthesian Point : %s\n", tmp_karthesian);
			
			//move xy plane down again
			tmp_karthesian.add(offset);
			//System.out.printf("Karthesian Point : %s\n", tmp_karthesian);
			
			//add to interpolated points pool
			points.add((Point3d) tmp_karthesian);
		}
		points.add(p2);
		
		return points;
	}
	
	protected void test() {
		Point3d_karthesian	point	= new Point3d_karthesian(10, 0, 0);
		Point3d_spherical	sphere	= point.toSpherePoint();
		Point3d_karthesian	reverse	= sphere.toPoint3d();
		
		for(int i = 0; i < 100; i++) {
			sphere = reverse.toSpherePoint();
			reverse = sphere.toPoint3d();
		}
		
		System.out.printf("Original point : %s\n", point);
		System.out.printf("Sphere   point : %s\n", sphere);
		System.out.printf("Reverse  point : %s\n", reverse);
		
		ArrayList<Point3d> points = new ArrayList<Point3d>();
		points.add((Point3d) point);
		points.add((Point3d) reverse);
		
		DebugVisualizer3D visual = new DebugVisualizer3D();
		int		pointSize				= 10;
		boolean	aliased					= true;
		boolean	showCoordinateSystem	= true;
		boolean	showSimulation			= false;
		visual.renderPointCloud(points, pointSize, aliased, showCoordinateSystem, showSimulation);
	}
}