vecmath3.cc

一个用MATLAB语言编写的摄像机标定工具箱,内容丰富

//
// vecmath3.cc
//
// $Id: vecmath3.cc,v 1.1.1.1 2001/02/28 00:28:40 cstolte Exp $
//

#include <sgl/vecmath3.h>
#include <math.h>

const Point3 Point3::origin(0,0,0);

/////////
// Point3

#define between_coord(val, low, high) ((val) >= (low) && (val) <= (high))

bool 
Point3::between(const Point3 &p1, const Point3 &p2) const {
    Point3 low = min(p1, p2);
    Point3 high = max(p1, p2);
    return (between_coord(x, low.x, high.x) &&
	    between_coord(y, low.y, high.y) &&
	    between_coord(z, low.z, high.z));
}

//////////
// Vector3

Vector3
Vector3::polar(double theta, double phi, double r) {
    phi -= M_PI;
    return Vector3(r * sin(theta) * cos(phi),
		   r * sin(theta) * sin(phi),
		   r * cos(theta));
}

void 
Vector3::fromPolar(double &theta, double &phi, double &len) const {
    len = length();
    theta = acos(z / len);
    phi = atan2(y / len, x / len);
    phi += M_PI;
}

double 
cos(const Vector3 &v1, const Vector3 &v2) {
    return dot(v1.normalize(), v2.normalize());
}

Vector3 
reflection(const Normal3 &n, const Vector3 &wi) {
    return -wi + 2.0 * dot(wi, n) * n.transpose();
}

int 
refraction(const Normal3 &n, const Vector3 &wi, Vector3 *wt,
	   double ni, double nt) {
    double eta = ni/nt;
    Vector3 i = -wi;
    double cosi = -dot(i, n);
    double cost = 1.0 - eta * eta * (1.0 - cosi * cosi);
    if (cost < 0.0)
        return 0;

    *wt = eta * i + (eta * cosi - sqrt(cost)) * n.transpose();
    return 1;
}

bool
Vector3::coordSystem(Vector3 *u, Vector3 *v) const
{
    *u = Vector3(-y, x, 0.);
    if (zero(u->length_squared())) {
	*u = Vector3(z, 0, -x);
	if (zero(u->length_squared()))
	    return false;
    }
    *u = u->hat();
    *v = cross(*this, *u).hat();
    return true;
}

//////////
// Normal3

double
cos(const Normal3 &n1, const Normal3 &n2) {
    return dot(n1.normalize(), n2.normalize());
}

double
cos(const Vector3 &v1, const Normal3 &n2) {
    return dot(v1.normalize(), n2.normalize());
}

double
cos(const Normal3 &n1, const Vector3 &v2) {
    return dot(n1.normalize(), v2.normalize());
}

bool
Normal3::coordSystem(Vector3 *u, Vector3 *v) const
{
    *u = Vector3(-y, x, 0.);
    if (zero(u->length_squared())) {
	*u = Vector3(z, 0, -x);
	if (zero(u->length_squared()))
	    return false;
    }
    *u = u->hat();
    *v = cross(Vector3(x, y, z), *u).hat();
    return true;
}

///////
// Ray3

std::ostream &
operator<<(std::ostream &os, const Ray3 &r)
{
   os << r.o << ' ' << r.d << std::endl;
   return os;
}