testvector3d.cc

torch tracking code, it is a good code

const char *help = "\
progname: testVector3D.cc\n\
code2html: This program tests Vector3D class methods.\n\
version: Torch3 vision2.0, 2004-2005\n\
(c) Dan Sunday (http:www.softsurfer.com) and Sebastien Marcel ([e-mail])\n";

#include "general.h"
#include "Point3D.h"
#include "Vector3D.h"
#include "CmdLine.h"

using namespace Torch;

int main(int argc, char **argv)
{
 	real x1, y1, z1;
 	real x2, y2, z2;
	bool verbose;

	// Construct the command line
  	CmdLine cmd;
	cmd.setBOption("write log", false);
	
  	// Put the help line at the beginning
  	cmd.info(help);

  	cmd.addText("\nArguments:");
  	cmd.addRCmdArg("x1", &x1, "x1");
  	cmd.addRCmdArg("y1", &y1, "y1");
  	cmd.addRCmdArg("z1", &z1, "z1");
  	cmd.addRCmdArg("x2", &x2, "x2");
  	cmd.addRCmdArg("y2", &y2, "y2");
  	cmd.addRCmdArg("z2", &z2, "z2");
  	cmd.addText("\nOptions:");
  	cmd.addBCmdOption("-verbose", &verbose, false, "verbose");

	cmd.read(argc, argv);

	double	 a, c[3];
	Vector3D u, v(1), w(0,1);
	Vector3D vx[3];


	// output preset coords of v and w
	print("Initial v = %s\n", v.sprint());
	print("Initial w = %s\n", w.sprint());

	// input new values 
	v.x = x1;
	v.y = y1;
	v.z = z1;

	w.x = x2;
	w.y = y2;
	w.z = z2;

	print("v = %s\n", v.sprint());
	print("w = %s\n", w.sprint());

	// test operators
	u = -v;			print("-v = %s\n", u.sprint());
	u = ~v;			print("~v = %s\n", u.sprint());
	u = 2*v;		print("2*v = %s\n", u.sprint());
	u = v + w;		print("v + w = %s\n", u.sprint());
	u = (v + w)/2;		print("(v + w)/2 = %s\n", u.sprint());
	u = v - w;		print("v - w = %s\n", u.sprint());
	u = (v - w)*0.5;	print("(v - w)*0.5 = %s\n", u.sprint());
	u = v ^ w;		print("v ^ w = %s\n", u.sprint());
	a = v * w;		print("v * w = %g\n", a);
	a = v | w;		print("v | w = %g\n", a);
	a = ~v * w;		print("~v * w = %g\n", a);
	print("\n");

	// test functions
	c[0]= 0.5; c[1]= 0.5;	print("c[]= {%g, %g}\n", c[0], c[1]);
	vx[0]= v;  vx[1]= w;	print("vx[]= {%s, %s}\n", vx[0].sprint(), vx[1].sprint());
	u = sum(2,c,vx);	print("sum(2,c,vx) = %s\n", u.sprint());

	u = v.len2();		print("v.len2() = %s\n", u.sprint());
	u = v.len();		print("v.len()  = %s\n", u.sprint());
	v.normalize();		print("v.normalize() = %s\n", v.sprint());
	w.normalize();		print("w.normalize() = %s\n", w.sprint());
	print("\n");

	// check that a few identities are always true
	int cnt=0, errs=0;
	Vector3D u0, uinc(2,0,-2);
	Vector3D v0, vinc(1,1,1);
	Vector3D w0, winc(0.2, 0.2, 0.0);
	Vector3D x, x0(-1,-2,-3), xinc(0.5,-0.5,0.1);
	double dl, dr;		// left and right side doubles
	Vector3D vl, vr;		// left and right side vectors
	double er, e=0.000000000001; // allow small float error e

	for (u=u0; u.x < 7; u+=uinc) 
	for (v=v0; v.x < 11; v+=vinc) 
	for (w=w0; w.x < 1.1; w+=winc) 
	{
		++cnt;
		// Triple product
		dl = u * (v ^ w);
		dr = (u ^ v) * w;
		er = abs(dl-dr);
		if (er > e) 
		{	// identity fails > e
			++errs;	// count errors
			warning("Error: err=%d\n", er);
		}
		++cnt;
		// Left Association of Cross Product
		vl = (u ^ v) ^ w;
		vr = (u * w) * v - (v * w) * u;
		er = (vl-vr).len();
		if (er > e) 
		{	// identity fails > e
			++errs;	// count errors
			warning("Error: err=%d\n", er);
		}
		++cnt;
		// Lagrange Identity
		for (x=x0; x.x < 1.1; x+=xinc) 
		{
			dl = (u ^ v) * (w ^ x);
			dr = (u * w) * (v * x) - (v * w) * (u * x);
			er = abs(dl-dr);
			if (er > e) 
			{	// identity fails > e
				++errs;	// count errors
				warning("Error: err=%d\n", er);
			}
			++cnt;
		}
	}
	print("Did %d identity chks. Had %d errors\n", cnt, errs);
}