nv_algebra.cpp

robocup rcssserver 运行防真机器人足球比赛所用的服务器端

	return ((nv_scalar)(rand() - HALF_RAND) / (nv_scalar)HALF_RAND);
}

// v is normalized
// theta in radians
void mat3::set_rot(const nv_scalar& theta, const vec3& v) 
{
    nv_scalar ct = nv_scalar(cos(theta));
    nv_scalar st = nv_scalar(sin(theta));

    nv_scalar xx = v.x * v.x;
    nv_scalar yy = v.y * v.y;
    nv_scalar zz = v.z * v.z;
    nv_scalar xy = v.x * v.y;
    nv_scalar xz = v.x * v.z;
    nv_scalar yz = v.y * v.z;

    a00 = xx + ct*(1-xx);
    a01 = xy + ct*(-xy) + st*-v.z;
    a02 = xz + ct*(-xz) + st*v.y;

    a10 = xy + ct*(-xy) + st*v.z;
    a11 = yy + ct*(1-yy);
    a12 = yz + ct*(-yz) + st*-v.x;

    a20 = xz + ct*(-xz) + st*-v.y;
    a21 = yz + ct*(-yz) + st*v.x;
    a22 = zz + ct*(1-zz);
}

void mat3::set_rot(const vec3& u, const vec3& v)
{
    nv_scalar phi;
    nv_scalar h;
    nv_scalar lambda;
    vec3 w;

    cross(w,u,v);
    dot(phi,u,v);
    dot(lambda,w,w);
    if (lambda > nv_eps)
        h = (nv_one - phi) / lambda;
    else
        h = lambda;
    
    nv_scalar hxy = w.x * w.y * h;
    nv_scalar hxz = w.x * w.z * h;
    nv_scalar hyz = w.y * w.z * h;

    a00 = phi + w.x * w.x * h;
    a01 = hxy - w.z;
    a02 = hxz + w.y;

    a10 = hxy + w.z;
    a11 = phi + w.y * w.y * h;
    a12 = hyz - w.x;

    a20 = hxz - w.y;
    a21 = hyz + w.x;
    a22 = phi + w.z * w.z * h;
}

void mat4::set_rot(const quat& q)
{
	mat3 m;
	q.ToMatrix(m);
	set_rot(m);
}

// v is normalized
// theta in radians
void mat4::set_rot(const nv_scalar& theta, const vec3& v) 
{
    nv_scalar ct = nv_scalar(cos(theta));
    nv_scalar st = nv_scalar(sin(theta));

    nv_scalar xx = v.x * v.x;
    nv_scalar yy = v.y * v.y;
    nv_scalar zz = v.z * v.z;
    nv_scalar xy = v.x * v.y;
    nv_scalar xz = v.x * v.z;
    nv_scalar yz = v.y * v.z;

    a00 = xx + ct*(1-xx);
    a01 = xy + ct*(-xy) + st*-v.z;
    a02 = xz + ct*(-xz) + st*v.y;

    a10 = xy + ct*(-xy) + st*v.z;
    a11 = yy + ct*(1-yy);
    a12 = yz + ct*(-yz) + st*-v.x;

    a20 = xz + ct*(-xz) + st*-v.y;
    a21 = yz + ct*(-yz) + st*v.x;
    a22 = zz + ct*(1-zz);
}

void mat4::set_rot(const vec3& u, const vec3& v)
{
    nv_scalar phi;
    nv_scalar h;
    nv_scalar lambda;
    vec3 w;

    cross(w,u,v);
    dot(phi,u,v);
    dot(lambda,w,w);
    if (lambda > nv_eps)
        h = (nv_one - phi) / lambda;
    else
        h = lambda;
    
    nv_scalar hxy = w.x * w.y * h;
    nv_scalar hxz = w.x * w.z * h;
    nv_scalar hyz = w.y * w.z * h;

    a00 = phi + w.x * w.x * h;
    a01 = hxy - w.z;
    a02 = hxz + w.y;

    a10 = hxy + w.z;
    a11 = phi + w.y * w.y * h;
    a12 = hyz - w.x;

    a20 = hxz - w.y;
    a21 = hyz + w.x;
    a22 = phi + w.z * w.z * h;
}

void mat4::set_rot(const mat3& M)
{
    // copy the 3x3 rotation block
    a00 = M.a00; a10 = M.a10; a20 = M.a20;
    a01 = M.a01; a11 = M.a11; a21 = M.a21;
    a02 = M.a02; a12 = M.a12; a22 = M.a22;
}

void mat4::set_translation(const vec3& t)
{
    a03 = t.x;
    a13 = t.y;
    a23 = t.z;
}

vec3 & mat4::get_translation(vec3& t) const
{
    t.x = a03;
    t.y = a13;
    t.z = a23;
    return t;
}

mat3 & mat4::get_rot(mat3& M) const
{
    // assign the 3x3 rotation block
    M.a00 = a00; M.a10 = a10; M.a20 = a20;
    M.a01 = a01; M.a11 = a11; M.a21 = a21;
    M.a02 = a02; M.a12 = a12; M.a22 = a22;
    return M;
}

quat & mat4::get_rot(quat& q) const
{
	mat3 m;
	get_rot(m);
	q.FromMatrix(m);
    return q;
}

mat3& tangent_basis(mat3& basis, const vec3& v0, const vec3& v1, const vec3& v2, const vec2& t0, const vec2& t1, const vec2& t2, const vec3 & n)
{
    vec3 cp;
    vec3 e0(v1.x - v0.x, t1.s - t0.s, t1.t - t0.t);
    vec3 e1(v2.x - v0.x, t2.s - t0.s, t2.t - t0.t);

    cross(cp,e0,e1);
    if ( fabs(cp.x) > nv_eps)
    {
        basis.a00 = -cp.y / cp.x;        
        basis.a10 = -cp.z / cp.x;
    }

    e0.x = v1.y - v0.y;
    e1.x = v2.y - v0.y;

    cross(cp,e0,e1);
    if ( fabs(cp.x) > nv_eps)
    {
        basis.a01 = -cp.y / cp.x;        
        basis.a11 = -cp.z / cp.x;
    }

    e0.x = v1.z - v0.z;
    e1.x = v2.z - v0.z;

    cross(cp,e0,e1);
    if ( fabs(cp.x) > nv_eps)
    {
        basis.a02 = -cp.y / cp.x;        
        basis.a12 = -cp.z / cp.x;
    }

    // tangent...
    nv_scalar oonorm = nv_one / sqrtf(basis.a00 * basis.a00 + basis.a01 * basis.a01 + basis.a02 * basis.a02);
    basis.a00 *= oonorm;
    basis.a01 *= oonorm;
    basis.a02 *= oonorm;

    // binormal...
    oonorm = nv_one / sqrtf(basis.a10 * basis.a10 + basis.a11 * basis.a11 + basis.a12 * basis.a12);
    basis.a10 *= oonorm;
    basis.a11 *= oonorm;
    basis.a12 *= oonorm;

    // normal...
    // compute the cross product TxB
    basis.a20 = basis.a01*basis.a12 - basis.a02*basis.a11;
    basis.a21 = basis.a02*basis.a10 - basis.a00*basis.a12;
    basis.a22 = basis.a00*basis.a11 - basis.a01*basis.a10;

    oonorm = nv_one / sqrtf(basis.a20 * basis.a20 + basis.a21 * basis.a21 + basis.a22 * basis.a22);
    basis.a20 *= oonorm;
    basis.a21 *= oonorm;
    basis.a22 *= oonorm;

    // Gram-Schmidt orthogonalization process for B
    // compute the cross product B=NxT to obtain 
    // an orthogonal basis
    basis.a10 = basis.a21*basis.a02 - basis.a22*basis.a01;
    basis.a11 = basis.a22*basis.a00 - basis.a20*basis.a02;
    basis.a12 = basis.a20*basis.a01 - basis.a21*basis.a00;

    if (basis.a20 * n.x + basis.a21 * n.y + basis.a22 * n.z < nv_zero)
    {
        basis.a20 = -basis.a20;
        basis.a21 = -basis.a21;
        basis.a22 = -basis.a22;
    }
    return basis;
}

/*
 * Project an x,y pair onto a sphere of radius r OR a hyperbolic sheet
 * if we are away from the center of the sphere.
 */
 nv_scalar tb_project_to_sphere(nv_scalar r, nv_scalar x, nv_scalar y)
{
    nv_scalar d, t, z;

    d = sqrtf(x*x + y*y);
    if (d < r * 0.70710678118654752440) {    /* Inside sphere */
        z = sqrtf(r*r - d*d);
    } else {           /* On hyperbola */
        t = r / (nv_scalar)1.41421356237309504880;
        z = t*t / d;
    }
    return z;
}

/*
 * Ok, simulate a track-ball.  Project the points onto the virtual
 * trackball, then figure out the axis of rotation, which is the cross
 * product of P1 P2 and O P1 (O is the center of the ball, 0,0,0)
 * Note:  This is a deformed trackball-- is a trackball in the center,
 * but is deformed into a hyperbolic sheet of rotation away from the
 * center.  This particular function was chosen after trying out
 * several variations.
 *
 * It is assumed that the arguments to this routine are in the range
 * (-1.0 ... 1.0)
 */
quat & trackball(quat& q, vec2& pt1, vec2& pt2, nv_scalar trackballsize)
{
    vec3 a; // Axis of rotation
    float phi;  // how much to rotate about axis
    vec3 d;
    float t;

    if (pt1.x == pt2.x && pt1.y == pt2.y) 
    {
        // Zero rotation
        q = quat_id;
        return q;
    }

    // First, figure out z-coordinates for projection of P1 and P2 to
    // deformed sphere
    vec3 p1(pt1.x,pt1.y,tb_project_to_sphere(trackballsize,pt1.x,pt1.y));
    vec3 p2(pt2.x,pt2.y,tb_project_to_sphere(trackballsize,pt2.x,pt2.y));

    //  Now, we want the cross product of P1 and P2
    cross(a,p1,p2);

    //  Figure out how much to rotate around that axis.
    d.x = p1.x - p2.x;
    d.y = p1.y - p2.y;
    d.z = p1.z - p2.z;
    t = sqrtf(d.x * d.x + d.y * d.y + d.z * d.z) / (trackballsize);

    // Avoid problems with out-of-control values...

    if (t > nv_one)
        t = nv_one;
    if (t < -nv_one) 
        t = -nv_one;
    phi = nv_two * nv_scalar(asin(t));
    axis_to_quat(q,a,phi);
    return q;
}

vec3& cube_map_normal(int i, int x, int y, int cubesize, vec3& v)
{
    nv_scalar s, t, sc, tc;
    s = (nv_scalar(x) + nv_zero_5) / nv_scalar(cubesize);
    t = (nv_scalar(y) + nv_zero_5) / nv_scalar(cubesize);
    sc = s * nv_two - nv_one;
    tc = t * nv_two - nv_one;

    switch (i) 
    {
        case 0:
            v.x = nv_one;
            v.y = -tc;
            v.z = -sc;
            break;
        case 1:
            v.x = -nv_one;
            v.y = -tc;
            v.z = sc;
            break;
        case 2:
            v.x = sc;
            v.y = nv_one;
            v.z = tc;
            break;
        case 3:
            v.x = sc;
            v.y = -nv_one;
            v.z = -tc;
            break;
        case 4:
            v.x = sc;
            v.y = -tc;
            v.z = nv_one;
            break;
        case 5:
            v.x = -sc;
            v.y = -tc;
            v.z = -nv_one;
            break;
    }
    normalize(v);
    return v;
}

// computes the area of a triangle
nv_scalar nv_area(const vec3& v1, const vec3& v2, const vec3& v3)
{
    vec3 cp_sum;
    vec3 cp;
    cross(cp_sum, v1, v2);
    cp_sum += cross(cp, v2, v3);
    cp_sum += cross(cp, v3, v1);
    return nv_norm(cp_sum) * nv_zero_5; 
}

// computes the perimeter of a triangle
nv_scalar nv_perimeter(const vec3& v1, const vec3& v2, const vec3& v3)
{
    nv_scalar perim;
    vec3 diff;
    sub(diff, v1, v2);
    perim = nv_norm(diff);
    sub(diff, v2, v3);
    perim += nv_norm(diff);
    sub(diff, v3, v1);
    perim += nv_norm(diff);
    return perim;
}

// compute the center and radius of the inscribed circle defined by the three vertices
nv_scalar nv_find_in_circle(vec3& center, const vec3& v1, const vec3& v2, const vec3& v3)
{
    nv_scalar area = nv_area(v1, v2, v3);
    // if the area is null
    if (area < nv_eps)
    {
        center = v1;
        return nv_zero;
    }

    nv_scalar oo_perim = nv_one / nv_perimeter(v1, v2, v3);

    vec3 diff;

    sub(diff, v2, v3);
    mult(center, v1, nv_norm(diff));

    sub(diff, v3, v1);
    madd(center, v2, nv_norm(diff));
    
    sub(diff, v1, v2);
    madd(center, v3, nv_norm(diff));

    center *= oo_perim;

    return nv_two * area * oo_perim;
}

// compute the center and radius of the circumscribed circle defined by the three vertices
// i.e. the osculating circle of the three vertices
nv_scalar nv_find_circ_circle( vec3& center, const vec3& v1, const vec3& v2, const vec3& v3)
{
    vec3 e0;
    vec3 e1;
    nv_scalar d1, d2, d3;
    nv_scalar c1, c2, c3, oo_c;

    sub(e0, v3, v1);
    sub(e1, v2, v1);
    dot(d1, e0, e1);

    sub(e0, v3, v2);
    sub(e1, v1, v2);
    dot(d2, e0, e1);

    sub(e0, v1, v3);
    sub(e1, v2, v3);
    dot(d3, e0, e1);

    c1 = d2 * d3;
    c2 = d3 * d1;
    c3 = d1 * d2;
    oo_c = nv_one / (c1 + c2 + c3);

    mult(center,v1,c2 + c3);
    madd(center,v2,c3 + c1);
    madd(center,v3,c1 + c2);
    center *= oo_c * nv_zero_5;
 
    return nv_zero_5 * sqrtf((d1 + d2) * (d2 + d3) * (d3 + d1) * oo_c);
}

 nv_scalar ffast_cos(const nv_scalar x)
{
    // assert:  0 <= fT <= PI/2
    // maximum absolute error = 1.1880e-03
    // speedup = 2.14

    nv_scalar x_sqr = x*x;
    nv_scalar res = nv_scalar(3.705e-02);
    res *= x_sqr;
    res -= nv_scalar(4.967e-01);
    res *= x_sqr;
    res += nv_one;
    return res;
}


 nv_scalar fast_cos(const nv_scalar x)
{
    // assert:  0 <= fT <= PI/2
    // maximum absolute error = 2.3082e-09
    // speedup = 1.47

    nv_scalar x_sqr = x*x;
    nv_scalar res = nv_scalar(-2.605e-07);
    res *= x_sqr;
    res += nv_scalar(2.47609e-05);
    res *= x_sqr;
    res -= nv_scalar(1.3888397e-03);
    res *= x_sqr;
    res += nv_scalar(4.16666418e-02);
    res *= x_sqr;
    res -= nv_scalar(4.999999963e-01);
    res *= x_sqr;
    res += nv_one;
    return res;
}

void nv_is_valid(const vec3& v)
{
    assert(!_isnan(v.x) && !_isnan(v.y) && !_isnan(v.z) &&
        _finite(v.x) && _finite(v.y) && _finite(v.z));
}

void nv_is_valid(nv_scalar lambda)
{
    assert(!_isnan(lambda) && _finite(lambda));
}