hmg.cc

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

//
// hmg.cc
//
// $Id: hmg.cc,v 1.1.1.1 2001/02/28 00:28:35 cstolte Exp $
//

#include <sgl/hmg.h>
#include <sgl/mathfunc.h>
#include <sgl/vecmath2.h>
#include <sgl/vecmath3.h>

///////////////
// Homogeneous2
///////////////

Homogeneous2::Homogeneous2(double u1, double v1, double d1) {
    u = u1;
    v = v1;
    d = d1;
}

Homogeneous2::Homogeneous2(const Point2 &p) {
    u = p.u;
    v = p.v;
    d = 1;
}

Homogeneous2::Homogeneous2(const Vector2 &v1) {
    u = v1.u;
    v = v1.v;
    d = 0;
}

Homogeneous2::operator Point2() const {
    assert(d != 0);
    return Point2(u / d, v / d);
}

Homogeneous2::operator Vector2() const {
    assert(d == 0);
    return Vector2(u, v);
}

Homogeneous2 Homogeneous2::operator-() const {
    return Homogeneous2(-u, -v, d);
}

Homogeneous2 Homogeneous2::operator+(const Homogeneous2 &h2) const {
    Homogeneous2 ret(*this);
    return ret += h2;
}

Homogeneous2 &Homogeneous2::operator+=(const Homogeneous2 &h2) {
    if (d == 0) {
	assert(h2.d == 0); // else we're doing vector + point--not kosher.
	// vector + vector
	u += h2.u;
	v += h2.v;
    }
    else {
	assert(h2.d == 0); // else it's point + point.
	// point + vector
	u += h2.u * d;
	v += h2.v * d;
    }
    return *this;
}

Homogeneous2 Homogeneous2::operator-(const Homogeneous2 &h2) const {
    Homogeneous2 ret(*this);
    return ret -= h2;
}

Homogeneous2 &Homogeneous2::operator-=(const Homogeneous2 &h2) {
    if (d == 0) {
	assert(h2.d == 0); // else it's vector - point
	// vector - vector
	u -= h2.u;
	v -= h2.v;
    }
    else {
	if (h2.d == 0) {
	    // point - vector.
	    u -= h2.u * d;
	    v -= h2.v * d;
	}
	else {
	    // point - point. become a vector.
	    u = (u/d) - (h2.u / h2.d);
	    v = (v/d) - (h2.v / h2.d);
	    d = 0;
	}
    }
    return *this;
}

Homogeneous2 Homogeneous2::operator*(double s) const {
    return Homogeneous2(u * s, v * s, d);
}

Homogeneous2 &Homogeneous2::operator*=(double s) {
    u *= s;
    v *= s;
    return *this;
}

Homogeneous2 Homogeneous2::operator/(double s) const {
    return Homogeneous2(u / s, v / s, d);
}

Homogeneous2 &Homogeneous2::operator/=(double s) {
    u /= s;
    v /= s;
    return *this;
}

bool Homogeneous2::operator==(const Homogeneous2 &h2) const {
    if (d == 0) {
	if (h2.d == 0)
	    return (u == h2.u && v == h2.v);
	else
	    return false;
    }
    else {
	if (h2.d == 0)
	    return false;
	double sc = h2.d / d;
	return ((u*sc) == h2.u && (v*sc) == h2.v);
    }
}

bool Homogeneous2::operator!=(const Homogeneous2 &h2) const {
    if (d == 0) {
	if (h2.d == 0)
	    return (u != h2.u || v != h2.v);
	else
	    return true;
    }
    else {
	if (h2.d == 0)
	    return true;

	double sc = h2.d / d;
	return ((u*sc) != h2.u || (v*sc) != h2.v);
    }
}

Homogeneous2 Homogeneous2::normalize() const {
    Homogeneous2 ret(*this);
    return ret /= length();
}

double Homogeneous2::length_squared() const {
    if (d == 0)
	return (u*u + v*v);
    else
	return ((u*u + v*v) / d*d);
}

////////////////
// Homogeneous3 
////////////////

Homogeneous3::Homogeneous3(double x1, double y1, double z1, double d1) {
    x = x1;
    y = y1;
    z = z1;
    d = d1;
}

Homogeneous3::Homogeneous3(const Point3 &p) {
    x = p.x;
    y = p.y;
    z = p.z;
    d = 1;
}

Homogeneous3::Homogeneous3(const Vector3 &v) {
    x = v.x;
    y = v.y;
    z = v.z;
    d = 0;
}

Homogeneous3::operator Point3() const {
    assert(d != 0);
    return Point3(x / d, y / d, z / d);
}

Homogeneous3::operator Vector3() const {
    assert(d == 0);
    return Vector3(x, y, z);
}

Homogeneous3 Homogeneous3::operator-() const {
    return Homogeneous3(-x, -y, -z, d);
}

Homogeneous3 Homogeneous3::operator+(const Homogeneous3 &h2) const {
    Homogeneous3 ret(*this);
    return ret += h2;
}

Homogeneous3 &Homogeneous3::operator+=(const Homogeneous3 &h2) {
    if (d == 0) {
	assert(h2.d == 0); // else we're doing vector + point--not kosher.
	// vector + vector
	x += h2.x;
	y += h2.y;
	z += h2.z;
    }
    else {
	assert(h2.d == 0); // else it's point + point.
	// point + vector
	x += h2.x * d;
	y += h2.y * d;
	z += h2.z * d;
    }
    return *this;
}

Homogeneous3 Homogeneous3::operator-(const Homogeneous3 &h2) const {
    Homogeneous3 ret(*this);
    return ret -= h2;
}

Homogeneous3 &Homogeneous3::operator-=(const Homogeneous3 &h2) {
    if (d == 0) {
	assert(h2.d == 0); // else it's vector - point
	// vector - vector
	x -= h2.x;
	y -= h2.y;
	z -= h2.z;
    }
    else {
	if (h2.d == 0) {
	    // point - vector.
	    x -= h2.x * d;
	    y -= h2.y * d;
	    z -= h2.z * d;
	}
	else {
	    // point - point. become a vector.
	    x = (x/d) - (h2.x / h2.d);
	    y = (y/d) - (h2.y / h2.d);
	    z = (z/d) - (h2.z / h2.d);
	    d = 0;
	}
    }
    return *this;
}

Homogeneous3 Homogeneous3::operator*(double s) const {
    return Homogeneous3(x*s, y*s, z*s, d);
}

Homogeneous3 &Homogeneous3::operator*=(double s) {
    x *= s;
    y *= s;
    z *= s;
    return *this;
}

Homogeneous3 Homogeneous3::operator/(double s) const {
    return Homogeneous3(x/s, y/s, z/s, d);
}

Homogeneous3 &Homogeneous3::operator/=(double s) {
    x /= s;
    y /= s;
    z /= s;
    return *this;
}

bool Homogeneous3::operator==(const Homogeneous3 &h2) const {
    if (d == 0) {
	if (h2.d == 0)
	    return (x == h2.x && y == h2.y && z == h2.z);
	else
	    return false;
    }
    else {
	if (h2.d == 0)
	    return false;
	double sc = h2.d / d;
	return ((x*sc) == h2.x && (y*sc) == h2.y && (z*sc) == h2.z);
    }
}

bool Homogeneous3::operator!=(const Homogeneous3 &h2) const {
    if (d == 0) {
	if (h2.d == 0)
	    return (x != h2.x || y != h2.y || z != h2.z);
	else
	    return true;
    }
    else {
	if (h2.d == 0)
	    return true;

	double sc = h2.d / d;
	return ((x*sc) != h2.x || (y*sc) != h2.y || (z*sc) != h2.z);
    }
}

Homogeneous3 Homogeneous3::normalize() const {
    Homogeneous3 ret(*this);
    return ret /= length();
}

double Homogeneous3::length_squared() const {
    if (d == 0)
	return (x*x + y*y + z*z);
    else
	return ((x*x + y*y + z*z) / d*d);
}