math.c

一个用于智能手机的多媒体库适合S60 WinCE的跨平台开发库

	if (fabs(det) > FIX_EPSILON) {
		Fixed fc0, fc1;
		GF_Vec v1, v2;
		fc0 = gf_divfix( gf_mulfix(fn11, -plane->d) + gf_mulfix(fn01, with->d) , det);
		fc1 = gf_divfix( gf_mulfix(fn00, -with->d) + gf_mulfix(fn01, plane->d) , det);
		*linevec = gf_vec_cross(plane->normal, with->normal);
		v1 = gf_vec_scale(plane->normal, fc0);
		v2 = gf_vec_scale(with->normal, fc1);
		gf_vec_add(*linepoint, v1, v2);
		return 1;
	}
	return 0;
}

GF_EXPORT
Bool gf_plane_intersect_planes(GF_Plane *plane, GF_Plane *p1, GF_Plane *p2, GF_Vec *outPoint)
{
	GF_Vec lp, lv;
	if (gf_plane_intersect_plane(plane, p1, &lp, &lv))
		return gf_plane_intersect_line(p2, &lp, &lv, outPoint);
	return 0;
}



GF_EXPORT
GF_Ray gf_ray(GF_Vec start, GF_Vec end)
{
	GF_Ray r;
	r.orig = start;
	gf_vec_diff(r.dir, end, start);
	gf_vec_norm(&r.dir);
	return r;
}

GF_EXPORT
void gf_mx_apply_ray(GF_Matrix *mx, GF_Ray *r)
{
	gf_vec_add(r->dir, r->orig, r->dir);
	gf_mx_apply_vec(mx, &r->orig);
	gf_mx_apply_vec(mx, &r->dir);
	gf_vec_diff(r->dir, r->dir, r->orig);
	gf_vec_norm(&r->dir);
}

#define XPLANE 0
#define YPLANE 1
#define ZPLANE 2


GF_EXPORT
Bool gf_ray_hit_box(GF_Ray *ray, GF_Vec box_min, GF_Vec box_max, GF_Vec *outPoint)
{
	Fixed t1, t2, tNEAR=FIX_MIN, tFAR=FIX_MAX;
	Fixed temp;
	s8 xyorz, sign;
	
	if (ray->dir.x == 0) {
		if ((ray->orig.x < box_min.x) || (ray->orig.x > box_max.x))
			return 0;
	} else {
		t1 = gf_divfix(box_min.x - ray->orig.x, ray->dir.x);
		t2 = gf_divfix(box_max.x - ray->orig.x, ray->dir.x);
		if (t1 > t2) {
			temp = t1;
			t1 = t2;
			t2 = temp;
		}
		if (t1 > tNEAR) {
			tNEAR = t1;
			xyorz = XPLANE;
			sign = (ray->dir.x < 0) ? 1 : -1;
		}
		if (t2 < tFAR) tFAR = t2;
		if (tNEAR > tFAR) return 0; // box missed
		if (tFAR < 0) return 0; // box behind the ray
	}

	if (ray->dir.y == 0) {
		if ((ray->orig.y < box_min.y) || (ray->orig.y > box_max.y)) 
			return 0;
	} else {
		t1 = gf_divfix(box_min.y - ray->orig.y, ray->dir.y);
		t2 = gf_divfix(box_max.y - ray->orig.y, ray->dir.y);
		if (t1 > t2) {
			temp = t1;
			t1 = t2;
			t2 = temp;
		}
		if (t1 > tNEAR) {
			tNEAR = t1;
			xyorz = YPLANE;
			sign = (ray->dir.y < 0) ? 1 : -1;
		}
		if (t2 < tFAR) tFAR = t2;
		if (tNEAR > tFAR) return 0; // box missed
		if (tFAR < 0) return 0; // box behind the ray
	}

	// Check the Z plane
	if (ray->dir.z == 0) {
		if ((ray->orig.z < box_min.z) || (ray->orig.z > box_max.z))
			return 0;
	} else {
		t1 = gf_divfix(box_min.z - ray->orig.z, ray->dir.z);
		t2 = gf_divfix(box_max.z - ray->orig.z, ray->dir.z);
		if (t1 > t2) {
			temp = t1;
			t1 = t2;
			t2 = temp;
		}
		if (t1 > tNEAR) {
			tNEAR = t1;
			xyorz = ZPLANE;
			sign = (ray->dir.z < 0) ? 1 : -1;
		}
		if (t2 < tFAR) tFAR = t2;
		if (tNEAR>tFAR) return 0; // box missed
		if (tFAR < 0) return 0;  // box behind the ray
	}
	if (outPoint) {
		*outPoint = gf_vec_scale(ray->dir, tNEAR);
		gf_vec_add(*outPoint, *outPoint, ray->orig);
	}
	return 1;
}


GF_EXPORT
Bool gf_ray_hit_sphere(GF_Ray *ray, GF_Vec *center, Fixed radius, GF_Vec *outPoint)
{
	GF_Vec radv;
	Fixed dist, center_proj, center_proj_sq, hcord;
	if (center) {
		gf_vec_diff(radv, *center, ray->orig);
	} else {
		radv = ray->orig;
		gf_vec_rev(radv);
	}
	dist = gf_vec_len(radv);
	center_proj = gf_vec_dot(radv, ray->dir);
	if (radius + ABS(center_proj) < dist ) return 0;
	
	center_proj_sq = gf_mulfix(center_proj, center_proj);
	hcord = center_proj_sq - gf_mulfix(dist, dist) + gf_mulfix(radius , radius);
    if (hcord < 0) return 0;
	if (center_proj_sq < hcord) return 0;
	if (outPoint) {
		center_proj -= gf_sqrt(hcord);
		radv = gf_vec_scale(ray->dir, center_proj);
		gf_vec_add(*outPoint, ray->orig, radv);
	}
	return 1;
}

/*
 *		Tomas M鰈ler and Ben Trumbore.
 *	 Fast, minimum storage ray-triangle intersection. 
 *		Journal of graphics tools, 2(1):21-28, 1997
 *
 */
GF_EXPORT
Bool gf_ray_hit_triangle(GF_Ray *ray, GF_Vec *v0, GF_Vec *v1, GF_Vec *v2, Fixed *dist)
{
	Fixed u, v, det;
	GF_Vec edge1, edge2, tvec, pvec, qvec;
	/* find vectors for two edges sharing vert0 */
	gf_vec_diff(edge1, *v1, *v0);
	gf_vec_diff(edge2, *v2, *v0);
	/* begin calculating determinant - also used to calculate U parameter */
	pvec = gf_vec_cross(ray->dir, edge2);
	/* if determinant is near zero, ray lies in plane of triangle */
	det = gf_vec_dot(edge1, pvec);
	if (ABS(det) < FIX_EPSILON) return 0;
	/* calculate distance from vert0 to ray origin */
	gf_vec_diff(tvec, ray->orig, *v0);
	/* calculate U parameter and test bounds */
	u = gf_divfix(gf_vec_dot(tvec, pvec), det);
	if ((u < 0) || (u > FIX_ONE)) return 0;
	/* prepare to test V parameter */
	qvec = gf_vec_cross(tvec, edge1);
	/* calculate V parameter and test bounds */
	v = gf_divfix(gf_vec_dot(ray->dir, qvec), det);
	if ((v < 0) || (u + v > FIX_ONE)) return 0;
	/* calculate t, ray intersects triangle */
	*dist = gf_divfix(gf_vec_dot(edge2, qvec), det);
	return 1;
}

GF_EXPORT
Bool gf_ray_hit_triangle_backcull(GF_Ray *ray, GF_Vec *v0, GF_Vec *v1, GF_Vec *v2, Fixed *dist)
{
	Fixed u, v, det;
	GF_Vec edge1, edge2, tvec, pvec, qvec;
	/* find vectors for two edges sharing vert0 */
	gf_vec_diff(edge1, *v1, *v0);
	gf_vec_diff(edge2, *v2, *v0);
	/* begin calculating determinant - also used to calculate U parameter */
	pvec = gf_vec_cross(ray->dir, edge2);
	/* if determinant is near zero, ray lies in plane of triangle */
	det = gf_vec_dot(edge1, pvec);
	if (det < FIX_EPSILON) return 0;
	/* calculate distance from vert0 to ray origin */
	gf_vec_diff(tvec, ray->orig, *v0);
	/* calculate U parameter and test bounds */
	u = gf_vec_dot(tvec, pvec);
	if ((u < 0) || (u > det)) return 0;
	/* prepare to test V parameter */
	qvec = gf_vec_cross(tvec, edge1);
	/* calculate V parameter and test bounds */
	v = gf_vec_dot(ray->dir, qvec);
	if ((v < 0) || (u + v > det)) return 0;
	/* calculate t, scale parameters, ray intersects triangle */
	*dist = gf_divfix(gf_vec_dot(edge2, qvec), det);
	return 1;
}

GF_EXPORT
GF_Vec gf_closest_point_to_line(GF_Vec line_pt, GF_Vec line_vec, GF_Vec pt)
{
	GF_Vec c;
	Fixed t;
	gf_vec_diff(c, pt, line_pt);
	t = gf_vec_dot(line_vec, c);
	c = gf_vec_scale(line_vec, t);
	gf_vec_add(c, c, line_pt);
	return c;
}


GF_EXPORT
GF_Vec4 gf_quat_from_matrix(GF_Matrix *mx)
{
	GF_Vec4 res;
	Fixed diagonal, s;
    diagonal = mx->m[0] + mx->m[5] + mx->m[10];

    if (diagonal > 0) {
        s = gf_sqrt(diagonal + FIX_ONE);
        res.q = s / 2;
        s = gf_invfix(2*s);
        res.x = gf_mulfix(mx->m[6] - mx->m[9], s);
        res.y = gf_mulfix(mx->m[8] - mx->m[2], s);
        res.z = gf_mulfix(mx->m[1] - mx->m[4], s);
    } else {
		Fixed q[4];
        u32 i, j, k;
        static const u32 next[3] = { 1, 2, 0 };
        i = 0;
        if (mx->m[5] > mx->m[0]) { i = 1; }
        if (mx->m[10] > mx->m[4*i+i]) { i = 2; }
        j = next[i];
        k = next[j];
        s = gf_sqrt(FIX_ONE + mx->m[4*i + i] - (mx->m[4*j+j] + mx->m[4*k+k]) );
        q[i] = s / 2;
        if (s != 0) { s = gf_invfix(2*s); }
        q[3] = gf_mulfix(mx->m[4*j+k] - mx->m[4*k+j], s);
        q[j] = gf_mulfix(mx->m[4*i+j] + mx->m[4*j+i], s);
        q[k] = gf_mulfix(mx->m[4*i+k] + mx->m[4*k+i], s);
		res.x = q[0]; res.y = q[1]; res.z = q[2]; res.q = q[3];
    }
	return res;
}

GF_EXPORT
GF_Vec4 gf_quat_to_rotation(GF_Vec4 *quat)
{
	GF_Vec4 r;
    Fixed val = gf_acos(quat->q);
    if (val == 0) {
        r.x = r.y = 0;
        r.z = FIX_ONE;
		r.q = 0;
    } else {
		GF_Vec axis;
        Fixed sin_val = gf_sin(val);
        axis.x = gf_divfix(quat->x, sin_val);
        axis.y = gf_divfix(quat->y, sin_val);
        axis.z = gf_divfix(quat->z, sin_val);
		gf_vec_norm(&axis);
		r.x = axis.x;
		r.y = axis.y;
		r.z = axis.z;
        r.q= 2 * val;
    }
	return r;
}

GF_EXPORT
GF_Vec4 gf_quat_from_rotation(GF_Vec4 rot)
{
	GF_Vec4 res;
	Fixed s;
	Fixed scale = gf_sqrt( gf_mulfix(rot.x, rot.x) + gf_mulfix(rot.y, rot.y) + gf_mulfix(rot.z, rot.z));

	/* no rotation - use (multiplication ???) identity quaternion */
	if (scale == 0) {
		res.q = FIX_ONE;
		res.x = 0;
		res.y = 0;
		res.z = 0;
	} else {
		s = gf_sin(rot.q/2);
		res.q = gf_cos(rot.q / 2);
		res.x = gf_muldiv(s, rot.x, scale);
		res.y = gf_muldiv(s, rot.y, scale);
		res.z = gf_muldiv(s, rot.z, scale);
		gf_quat_norm(res);
	}
	return res;
}

GF_EXPORT
GF_Vec4 gf_quat_from_axis_cos(GF_Vec axis, Fixed cos_a)
{
	GF_Vec4 r;
	if (cos_a < -FIX_ONE) cos_a = -FIX_ONE;
	else if (cos_a > FIX_ONE) cos_a = FIX_ONE;
	r.x = axis.x; r.y = axis.y; r.z = axis.z;
	r.q = gf_acos(cos_a);
	return gf_quat_from_rotation(r);
}

static void gf_quat_conjugate(GF_Vec4 *quat)
{
	quat->x *= -1;
	quat->y *= -1;
	quat->z *= -1;
}

GF_EXPORT
GF_Vec4 gf_quat_get_inv(GF_Vec4 *quat)
{
	GF_Vec4 ret = *quat;
	gf_quat_conjugate(&ret);
	gf_quat_norm(ret);
	return ret;
}


GF_EXPORT
GF_Vec4 gf_quat_multiply(GF_Vec4 *q1, GF_Vec4 *q2)
{
	GF_Vec4 ret;
	ret.q = gf_mulfix(q1->q, q2->q) - gf_mulfix(q1->x, q2->x) - gf_mulfix(q1->y, q2->y) - gf_mulfix(q1->z, q2->z);
	ret.x = gf_mulfix(q1->q, q2->x) + gf_mulfix(q1->x, q2->q) + gf_mulfix(q1->y, q2->z) - gf_mulfix(q1->z, q2->y);
	ret.y = gf_mulfix(q1->q, q2->y) + gf_mulfix(q1->y, q2->q) - gf_mulfix(q1->x, q2->z) + gf_mulfix(q1->z, q2->x);
	ret.z = gf_mulfix(q1->q, q2->z) + gf_mulfix(q1->z, q2->q) + gf_mulfix(q1->x, q2->y) - gf_mulfix(q1->y, q2->x);
	return ret;
}

GF_EXPORT
GF_Vec gf_quat_rotate(GF_Vec4 *quat, GF_Vec *vec)
{
	GF_Vec ret;
	GF_Vec4 q_v, q_i, q_r1, q_r2;
	q_v.q = 0;
	q_v.x = vec->x;
	q_v.y = vec->y;
	q_v.z = vec->z;
	q_i = gf_quat_get_inv(quat);
	q_r1 = gf_quat_multiply(&q_v, &q_i);
	q_r2 = gf_quat_multiply(quat, &q_r1);
	ret.x = q_r2.x;
	ret.y = q_r2.y;
	ret.z = q_r2.z;
	return ret;
}

/*
 * Code from www.gamasutra.com/features/19980703/quaternions_01.htm,
 * Listing 5.
 *
 * SLERP(p, q, t) = [p sin((1 - t)a) + q sin(ta)] / sin(a)
 *
 * where a is the arc angle, quaternions pq = cos(q) and 0 <= t <= 1
 */
GF_EXPORT
GF_Vec4 gf_quat_slerp(GF_Vec4 q1, GF_Vec4 q2, Fixed frac)
{
	GF_Vec4 res;
	Fixed omega, cosom, sinom, scale0, scale1, q2_array[4];

	cosom = gf_mulfix(q1.x, q2.x) + gf_mulfix(q1.y, q2.y) + gf_mulfix(q1.z, q2.z) + gf_mulfix(q1.q, q2.q);
	if (cosom < 0) {
		cosom = -cosom;
		q2_array[0] = -q2.x;
		q2_array[1] = -q2.y;
		q2_array[2] = -q2.z;
		q2_array[3] = -q2.q;
	} else {
		q2_array[0] = q2.x;
		q2_array[1] = q2.y;
		q2_array[2] = q2.z;
		q2_array[3] = q2.q;
	}

	/* calculate coefficients */
	if ((FIX_ONE - cosom) > FIX_EPSILON) {
		omega = gf_acos(cosom);
		sinom = gf_sin(omega);
		scale0 = gf_divfix(gf_sin( gf_mulfix(FIX_ONE - frac, omega)), sinom);
		scale1 = gf_divfix(gf_sin( gf_mulfix(frac, omega)), sinom);
	} else {
		/* q1 & q2 are very close, so do linear interpolation */
		scale0 = FIX_ONE - frac;
		scale1 = frac;
	}
	res.x = gf_mulfix(scale0, q1.x) + gf_mulfix(scale1, q2_array[0]);
	res.y = gf_mulfix(scale0, q1.y) + gf_mulfix(scale1, q2_array[1]);
	res.z = gf_mulfix(scale0, q1.z) + gf_mulfix(scale1, q2_array[2]);
	res.q = gf_mulfix(scale0, q1.q) + gf_mulfix(scale1, q2_array[3]);
	return res;
}


/*update center & radius & is_set flag*/
GF_EXPORT
void gf_bbox_refresh(GF_BBox *b)
{
	GF_Vec v;
	gf_vec_add(v, b->min_edge, b->max_edge);
	b->center = gf_vec_scale(v, FIX_ONE / 2);
	gf_vec_diff(v, b->max_edge, b->min_edge);
	b->radius = gf_vec_len(v) / 2;
	b->is_set = 1;
}

GF_EXPORT
void gf_bbox_from_rect(GF_BBox *box, GF_Rect *rc)
{
	box->min_edge.x = rc->x;
	box->min_edge.y = rc->y - rc->height;
	box->min_edge.z = 0;
	box->max_edge.x = rc->x + rc->width;
	box->max_edge.y = rc->y;
	box->max_edge.z = 0;
	gf_bbox_refresh(box);
}

GF_EXPORT
void gf_rect_from_bbox(GF_Rect *rc, GF_BBox *box)
{
	rc->x = box->min_edge.x;
	rc->y = box->max_edge.y;
	rc->width = box->max_edge.x - box->min_edge.x;
	rc->height = box->max_edge.y - box->min_edge.y;
}

GF_EXPORT
void gf_bbox_grow_point(GF_BBox *box, GF_Vec pt)
{
	if (pt.x > box->max_edge.x) box->max_edge.x = pt.x;
	if (pt.y > box->max_edge.y) box->max_edge.y = pt.y;
	if (pt.z > box->max_edge.z) box->max_edge.z = pt.z;
	if (pt.x < box->min_edge.x) box->min_edge.x = pt.x;
	if (pt.y < box->min_edge.y) box->min_edge.y = pt.y;
	if (pt.z < box->min_edge.z) box->min_edge.z = pt.z;
}

GF_EXPORT
void gf_bbox_union(GF_BBox *b1, GF_BBox *b2)
{
	if (b2->is_set) {
		if (!b1->is_set) {
			*b1 = *b2;
		} else {
			gf_bbox_grow_point(b1, b2->min_edge);
			gf_bbox_grow_point(b1, b2->max_edge);
			gf_bbox_refresh(b1);
		}
	}
}

GF_EXPORT
Bool gf_bbox_equal(GF_BBox *b1, GF_BBox *b2)
{
	return (gf_vec_equal(b1->min_edge, b2->min_edge) && gf_vec_equal(b1->max_edge, b2->max_edge));
}

GF_EXPORT
Bool gf_bbox_point_inside(GF_BBox *box, GF_Vec *p)
{
	return (p->x >= box->min_edge.x && p->x <= box->max_edge.x &&
			p->y >= box->min_edge.y && p->y <= box->max_edge.y &&
			p->z >= box->min_edge.z && p->z <= box->max_edge.z);
}

/*vertices are ordered to respect p vertex indexes (vertex from bbox closer to plane)
and so that n-vertex (vertex from bbox farther from plane) is 7-p_vx_idx*/
GF_EXPORT
void gf_bbox_get_vertices(GF_Vec bmin, GF_Vec bmax, GF_Vec *vecs)
{
	vecs[0].x = vecs[1].x = vecs[2].x = vecs[3].x = bmax.x;
	vecs[4].x = vecs[5].x = vecs[6].x = vecs[7].x = bmin.x;
	vecs[0].y = vecs[1].y = vecs[4].y = vecs[5].y = bmax.y;
	vecs[2].y = vecs[3].y = vecs[6].y = vecs[7].y = bmin.y;
	vecs[0].z = vecs[2].z = vecs[4].z = vecs[6].z = bmax.z;
	vecs[1].z = vecs[3].z = vecs[5].z = vecs[7].z = bmin.z;
}


GF_EXPORT
void gf_mx_apply_plane(GF_Matrix *mx, GF_Plane *plane)
{
	GF_Vec pt, end;
	/*get pt*/
	pt = gf_vec_scale(plane->normal, -plane->d);
	gf_vec_add(end, pt, plane->normal);
	gf_mx_apply_vec(mx, &pt);
	gf_mx_apply_vec(mx, &end);
	gf_vec_diff(plane->normal, end, pt);
	gf_vec_norm(&plane->normal);
	plane->d = - gf_vec_dot(pt, plane->normal);
}

GF_EXPORT
Fixed gf_plane_get_distance(GF_Plane *plane, GF_Vec *p)
{
	return gf_vec_dot(*p, plane->normal) + plane->d;
}


/*return p-vertex index (vertex from bbox closer to plane) - index range from 0 to 8*/
GF_EXPORT
u32 gf_plane_get_p_vertex_idx(GF_Plane *p)
{
	if (p->normal.x>=0) {
		if (p->normal.y>=0) return (p->normal.z>=0) ? 0 : 1;
		return (p->normal.z>=0) ? 2 : 3;
	} else {
		if (p->normal.y>=0) return (p->normal.z>=0) ? 4 : 5;
		return (p->normal.z>=0) ? 6 : 7;
	}
}


GF_EXPORT
u32 gf_bbox_plane_relation(GF_BBox *box, GF_Plane *p)
{
	GF_Vec nearv, farv;
	nearv = box->max_edge;
	farv = box->min_edge;
	if (p->normal.x > 0) {
		nearv.x = box->min_edge.x;
		farv.x = box->max_edge.x;
	}
	if (p->normal.y > 0) {
		nearv.y = box->min_edge.y;
		farv.y = box->max_edge.y;
	}
	if (p->normal.z > 0) {
		nearv.z = box->min_edge.z;
		farv.z = box->max_edge.z;
	}
	if (gf_vec_dot(p->normal, nearv) + p->d > 0) return GF_BBOX_FRONT;
	if (gf_vec_dot(p->normal, farv) + p->d > 0) return GF_BBOX_INTER;
	return GF_BBOX_BACK;
}