mathfunc.cc

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

//
// mathfunc.cc
//
// $Id: mathfunc.cc,v 1.1.1.1 2001/02/28 00:28:37 cstolte Exp $
//

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

Point2
lerp(const Point2 &p1, const Point2 &p2, double t) {
    return Point2(((1.-t) * p1.u + t * p2.u),
		  ((1.-t) * p1.v + t * p2.v));
}

Point3
lerp(const Point3 &p1, const Point3 &p2, double t) {
    return Point3(((1.-t) * p1.x + t * p2.x),
		  ((1.-t) * p1.y + t * p2.y),
		  ((1.-t) * p1.z + t * p2.z));
}

Vector2
lerp(const Vector2 &p1, const Vector2 &p2, double t) {
    return Vector2(((1.-t) * p1.u + t * p2.u),
		   ((1.-t) * p1.v + t * p2.v));
}

Vector3
lerp(const Vector3 &p1, const Vector3 &p2, double t) {
    return Vector3(((1.-t) * p1.x + t * p2.x),
		   ((1.-t) * p1.y + t * p2.y),
		   ((1.-t) * p1.z + t * p2.z));
}

Normal2
lerp(const Normal2 &p1, const Normal2 &p2, double t) {
    return Normal2(((1.-t) * p1.u + t * p2.u),
		   ((1.-t) * p1.v + t * p2.v));
}

Normal3
lerp(const Normal3 &p1, const Normal3 &p2, double t) {
    return Normal3(((1.-t) * p1.x + t * p2.x),
		   ((1.-t) * p1.y + t * p2.y),
		   ((1.-t) * p1.z + t * p2.z));
}

int
cube(int s)
{
  return s * s * s;
}

u_long
cube(u_long s)
{
    return s * s * s;
}

double
cube( double f )
{
  return f * f * f;
}

int
even( int i )
{
  return !( i % 2 );
}

int
odd( int i )
{
  return i % 2;
}

int
quadratic(float a, float b, float c, float *t, float mint)
{
    float d = b * b - 4 * a * c;
    if (d < 0.)
        return 0;

    d = sqrt(d);
    float s = (b - d)/(2*a);
    if (s > mint && s < *t) {
	*t = s;
        return 1;
    }
    s = (b + d)/(2*a);
    if (s > mint && s < *t) {
	*t = s;
        return 1;
    }
    return 0;
}

int
quadratic(double a, double b, double c, double *t, double mint)
{
    double d = b * b - 4 * a * c;
    if (d < 0.)
        return 0;

    d = sqrt(d);
    double s = (b - d)/(2*a);
    if (s > mint && s < *t) {
	*t = s;
        return 1;
    }
    s = (b + d)/(2*a);
    if (s > mint && s < *t) {
	*t = s;
        return 1;
    }
    return 0;
}

int
gcd(int n, int m)
{
    while (n > 0) {
	int remainder = m % n;
	m = n;
	n = remainder;
    }
    return m;
}

int
lcm(int x, int y)
{
    return (x * y) / gcd(x, y);
}

double
mod(double a, double b) {
    int n = int(a/b);
    a -= n*b;
    if (a < 0)
        a += b;
    return a;
}

float
mod(float a, float b) {
    int n = int(a/b);
    a -= n*b;
    if (a < 0)
        a += b;
    return a;
}

int
mod(int a, int b) {
    int n = int(a/b);
    a -= n*b;
    if (a < 0)
        a += b;
    return a;
}

u_int
next_prime(u_int start)
{
  if (start <= 3)
      return 3;

  if ((start % 2) == 0)
      ++start;
  for (;;start+=2)
    {
      int stop = int(ceil(sqrt(start)));
      int check = 3;
      while (check <= stop && ((start % check) != 0))
          check += 2;
      if (check > stop)
          return start;
    }
}

double
bias(double a, double b) {
    return pow(a, log(b) / log(0.5));
}

double
gain(double a, double b) {
    if (a < .00001)
	return 0.;
    else if (a > .99999)
	return 1.;

    double p = log(1. - b) / log(0.5);
    if (a < 0.5)
	return pow(2 * a, p) / 2;
    else
	return 1. - pow(2 * (1. - a), p) / 2;
}

double
gammacorrect(double gamma, double x) {
    return pow(x, 1. / gamma);
}

#if 0
RGB
gammacorrect(double gamma, const RGB &color) {
    return RGB(pow(color.r, 1. / gamma),
	       pow(color.g, 1. / gamma),
	       pow(color.b, 1. / gamma));
}
#endif

Vector3
faceforward(const Vector3 &V, const Vector3 &R)
{
    if (dot(V, R) < 0)
	return V;
    else
	return -V;
}

Normal3
faceforward(const Normal3 &V, const Vector3 &R) {
    if (dot(V, R) < 0)
	return V;
    else
	return -V;
}

Vector3
faceforward(const Vector3 &V, const Normal3 &R) {
    if (dot(V, R) < 0)
	return V;
    else
	return -V;
}

int
step(double min, double val) {
    if (val < min)
	return 0;
    else
	return 1;
}

int
pulse(double min, double max, double val) {
    return (step(min, val) - step(max, val));
}

double
boxstep(double min, double max, double val) {
    if (val < min)
	return 0;
    else if (val > max)
	return 1;
    else 
	return (val - min) / (max - min);
}

double
smoothstep(double min, double max, double val) {
    if (val < min)
	return 0;
    else if (val >= max)
	return 1;
    else {
	double t = double(val - min) / double(max - min);
	return t * t * (3. - 2. * t);
    }
}

//
// Catmull-Rom spline:
//      | -1  3 -3  1 | | P_i-3 |
// .5 * |  2 -5  4 -1 | | P_i-2 |
//      | -1  0  1  0 | | P_i-1 |
//      |  0  2  0  0 | | P_i   |
//

static double cr_matrix[4][4] = 
  { { -.5,  1.5, -1.5,  .5 }, 
    {   1, -2.5,    2, -.5 },
    { -.5,    0,   .5,   0 },
    {   0,    1,    0,   0 } };

double spline(double t, const double *pts, int n_pts) {
    if (t == 0.)
	return pts[1];
    if (t == 1.)
	return pts[n_pts-2];
    else {
	double scale = t * (n_pts - 3);
	int first = (int)floor(scale);
	t = scale - first;

	double pi3 = pts[first];
	double pi2 = pts[first+1];
	double pi1 = pts[first+2];
	double pi0 = pts[first+3];

	double t2 = t * t;
	double t3 = t * t2;

	double r1 = t3 * cr_matrix[0][0] + t2 * cr_matrix[1][0] +
	    t * cr_matrix[2][0] + cr_matrix[3][0];
	double r2 = t3 * cr_matrix[0][1] + t2 * cr_matrix[1][1] +
	    t * cr_matrix[2][1] + cr_matrix[3][1];
	double r3 = t3 * cr_matrix[0][2] + t2 * cr_matrix[1][2] +
	    t * cr_matrix[2][2] + cr_matrix[3][2];
	double r4 = t3 * cr_matrix[0][3] + t2 * cr_matrix[1][3] +
	    t * cr_matrix[2][3] + cr_matrix[3][3];

	return r1 * pi3 + r2 * pi2 + r3 * pi1 + r4 * pi0;
    }
}

#if 0
RGB spline(double t, const RGB *pts, int n_pts) {
    if (t == 0.)
	return pts[1];
    if (t == 1.)
	return pts[n_pts-2];
    else {
	double scale = t * (n_pts - 3);
	int first = (int)floor(scale);
	t = scale - first;

	RGB pi3 = pts[first];
	RGB pi2 = pts[first+1];
	RGB pi1 = pts[first+2];
	RGB pi0 = pts[first+3];

	double t2 = t * t;
	double t3 = t * t2;

	double r1 = t3 * cr_matrix[0][0] + t2 * cr_matrix[1][0] +
	    t * cr_matrix[2][0] + cr_matrix[3][0];
	double r2 = t3 * cr_matrix[0][1] + t2 * cr_matrix[1][1] +
	    t * cr_matrix[2][1] + cr_matrix[3][1];
	double r3 = t3 * cr_matrix[0][2] + t2 * cr_matrix[1][2] +
	    t * cr_matrix[2][2] + cr_matrix[3][2];
	double r4 = t3 * cr_matrix[0][3] + t2 * cr_matrix[1][3] +
	    t * cr_matrix[2][3] + cr_matrix[3][3];

	return r1 * pi3 + r2 * pi2 + r3 * pi1 + r4 * pi0;
    }
}
#endif

Vector3 spline(double t, const Vector3 *pts, int n_pts) {
    if (t == 0.)
	return pts[1];
    if (t == 1.)
	return pts[n_pts-2];
    else {
	double scale = t * (n_pts - 3);
	int first = (int)floor(scale);
	t = scale - first;
	
	Vector3 pi3 = pts[first];
	Vector3 pi2 = pts[first+1];
	Vector3 pi1 = pts[first+2];
	Vector3 pi0 = pts[first+3];

	double t2 = t * t;
	double t3 = t * t2;

	double r1 = t3 * cr_matrix[0][0] + t2 * cr_matrix[1][0] +
	    t * cr_matrix[2][0] + cr_matrix[3][0];
	double r2 = t3 * cr_matrix[0][1] + t2 * cr_matrix[1][1] +
	    t * cr_matrix[2][1] + cr_matrix[3][1];
	double r3 = t3 * cr_matrix[0][2] + t2 * cr_matrix[1][2] +
	    t * cr_matrix[2][2] + cr_matrix[3][2];
	double r4 = t3 * cr_matrix[0][3] + t2 * cr_matrix[1][3] +
	    t * cr_matrix[2][3] + cr_matrix[3][3];

	return r1 * pi3 + r2 * pi2 + r3 * pi1 + r4 * pi0;
    }
}

Normal3 spline(double t, const Normal3 *pts, int n_pts) {
    if (t == 0.)
	return pts[1];
    if (t == 1.)
	return pts[n_pts-2];
    else {
	double scale = t * (n_pts - 3);
	int first = (int)floor(scale);
	t = scale - first;

	Normal3 pi3 = pts[first];
	Normal3 pi2 = pts[first+1];
	Normal3 pi1 = pts[first+2];
	Normal3 pi0 = pts[first+3];

	double t2 = t * t;
	double t3 = t * t2;

	double r1 = t3 * cr_matrix[0][0] + t2 * cr_matrix[1][0] +
	    t * cr_matrix[2][0] + cr_matrix[3][0];
	double r2 = t3 * cr_matrix[0][1] + t2 * cr_matrix[1][1] +
	    t * cr_matrix[2][1] + cr_matrix[3][1];
	double r3 = t3 * cr_matrix[0][2] + t2 * cr_matrix[1][2] +
	    t * cr_matrix[2][2] + cr_matrix[3][2];
	double r4 = t3 * cr_matrix[0][3] + t2 * cr_matrix[1][3] +
	    t * cr_matrix[2][3] + cr_matrix[3][3];

	return r1 * pi3 + r2 * pi2 + r3 * pi1 + r4 * pi0;
    }
}

static double
findPtLineT(const Point3 &l0, const Point3 &l1, const Point3 &p)
{
    double t3 = l0.x*l1.x;
    double t4 = l0.x*l0.x;
    double t7 = l0.y*l1.y;
    double t8 = l0.y*l0.y;
    double t11 = l0.z*l1.z;
    double t12 = l0.z*l0.z;
    double t13 = -p.x*l1.x+p.x*l0.x+t3-t4-p.y*l1.y+p.y*l0.y+t7-t8-p.z*l1.z+
	p.z*l0.z+t11-t12;
    double t14 = l1.x*l1.x;
    double t15 = l1.y*l1.y;
    double t16 = l1.z*l1.z;
    return -t13/(t14-2.0*t3+t4+t15-2.0*t7+t8+t16-2.0*t11+t12);
}

double
ptlined(const Point3 &l0, const Point3 &l1, const Point3 &p)
{
    double t = findPtLineT(l0, l1, p);
    return distance(p, l0 + t * (l1 - l0));
}

double
ptlined2(const Point3 &l0, const Point3 &l1, const Point3 &p)
{
    double t = findPtLineT(l0, l1, p);
    return distance_squared(p, l0 + t * (l1 - l0));
}

double
ptsegd(const Point3 &l0, const Point3 &l1, const Point3 &p)
{
    double t = findPtLineT(l0, l1, p);
    t = clamp(t, 0., 1.);
    return distance(p, l0 + t * (l1 - l0));
}

double
ptsegd2(const Point3 &l0, const Point3 &l1, const Point3 &p)
{
    double t = findPtLineT(l0, l1, p);
    t = clamp(t, 0., 1.);
    return distance_squared(p, l0 + t * (l1 - l0));
}