plane.cc

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

//
// plane.cc
//
// $Id: plane.cc,v 1.1.1.1 2001/02/28 00:28:38 cstolte Exp $
//

#include <sgl/plane.h>

template <class T>
inline void swap(T& a, T& b) {
    T tmp = a;
    a = b;
    b = tmp;
}

Plane::Plane(const Point3 &p, const Normal3 &n)
{
    init(p, n);
}

Plane::Plane(const Point3 &p, const Point3 &p1, const Point3 &p2)
{
    init(p, cross(p1 - p, p2 - p).transpose());
}

Plane::Plane(const Point3 &p, const Vector3 &v1, const Vector3 &v2)
{
    init(p, cross(v1, v2).transpose());
}

void
Plane::init(const Point3 &p, const Normal3 &n)
{
    a = n.x;
    b = n.y;
    c = n.z;
    d = -(p.x * n.x + p.y * n.y + p.z * n.z);
}

bool 
Plane::intersect(const Point3 &p, const Vector3 &dir, double t0, double *t) const
{
    double tt = -((p.x * a + p.y * b + p.z * c + d) /
		  (dir.x * a + dir.y * b + dir.z * c));

    if (tt > t0 && tt < *t) {
	*t = tt;
	return true;
    }
    else
	return false;
}

bool
Plane::line(const Plane &plane2, Point3 *pt, Vector3 *dir) const
{
    // Graphics Gems III, page 233.
    Vector3 N1(a, b, c), N2(plane2.a, plane2.b, plane2.c);
    *dir = cross(N1, N2);

    if (dir->length_squared() < 1e-5)
	return false;

    // Find component of the direction with largest component.
    double maxD = max(fabs(dir->x), fabs(dir->y), fabs(dir->z));
    int maxComponent = 0;

    if (fabs(dir->x) == maxD) {
	maxComponent = 1;
	swap(dir->x, dir->z);
	swap(N1.x, N1.z);
	swap(N2.x, N2.z);
    }
    else if (fabs(dir->y) == maxD) {
	maxComponent = 2;
	swap(dir->y, dir->z);
	swap(N1.y, N1.z);
	swap(N2.y, N2.z);
    }
    else {
	assert(fabs(dir->z) == maxD);
	maxComponent = 3;

	// wtf? I really don't want to know why this was necessary
	// to get the tests to pass.
	*dir *= -1.;
    }

    pt->x = (N1.y * -plane2.d - N2.y * -d) / dir->z;
    pt->y = (N2.x * -d - N1.x * -plane2.d) / dir->z;
    pt->z = 0;

    if (maxComponent == 1) {
	swap(dir->x, dir->z);
	swap(pt->x, pt->z);
    }
    else if (maxComponent == 2) {
	swap(dir->y, dir->z);
	swap(pt->y, pt->z);
    }

    return true;
}

std::ostream &
operator<<(std::ostream &os, const Plane &plane)
{
    os << plane.a << ' ' << plane.b << ' ' << plane.c << ' ' << plane.d;
    return os;
}