nuclear@0: #include <float.h>
nuclear@12: #include <assert.h>
nuclear@5: #include <algorithm>
nuclear@0: #include "curve.h"
nuclear@0: 
nuclear@0: Curve::Curve(CurveType type)
nuclear@0: {
nuclear@0: 	this->type = type;
nuclear@12: 	bbvalid = true;
nuclear@12: }
nuclear@12: 
nuclear@12: Curve::Curve(const Vector4 *cp, int numcp, CurveType type)
nuclear@12: 	: Curve(type)
nuclear@12: {
nuclear@12: 	this->cp.resize(numcp);
nuclear@12: 	for(int i=0; i<numcp; i++) {
nuclear@12: 		this->cp[i] = cp[i];
nuclear@12: 	}
nuclear@12: }
nuclear@12: 
nuclear@12: Curve::Curve(const Vector3 *cp, int numcp, CurveType type)
nuclear@12: 	: Curve(type)
nuclear@12: {
nuclear@12: 	this->cp.resize(numcp);
nuclear@12: 	for(int i=0; i<numcp; i++) {
nuclear@12: 		this->cp[i] = Vector4(cp[i].x, cp[i].y, cp[i].z, 1.0f);
nuclear@12: 	}
nuclear@12: }
nuclear@12: 
nuclear@12: Curve::Curve(const Vector2 *cp, int numcp, CurveType type)
nuclear@12: 	: Curve(type)
nuclear@12: {
nuclear@12: 	this->cp.resize(numcp);
nuclear@12: 	for(int i=0; i<numcp; i++) {
nuclear@12: 		this->cp[i] = Vector4(cp[i].x, cp[i].y, 0.0f, 1.0f);
nuclear@12: 	}
nuclear@0: }
nuclear@0: 
nuclear@0: void Curve::set_type(CurveType type)
nuclear@0: {
nuclear@0: 	this->type = type;
nuclear@0: }
nuclear@0: 
nuclear@0: CurveType Curve::get_type() const
nuclear@0: {
nuclear@0: 	return type;
nuclear@0: }
nuclear@0: 
nuclear@12: void Curve::add_point(const Vector4 &p)
nuclear@12: {
nuclear@12: 	cp.push_back(p);
nuclear@12: 	inval_bounds();
nuclear@12: }
nuclear@12: 
nuclear@12: void Curve::add_point(const Vector3 &p, float weight)
nuclear@12: {
nuclear@12: 	add_point(Vector4(p.x, p.y, p.z, weight));
nuclear@12: }
nuclear@12: 
nuclear@0: void Curve::add_point(const Vector2 &p, float weight)
nuclear@0: {
nuclear@12: 	add_point(Vector4(p.x, p.y, 0.0f, weight));
nuclear@0: }
nuclear@0: 
nuclear@0: bool Curve::remove_point(int idx)
nuclear@0: {
nuclear@0: 	if(idx < 0 || idx >= (int)cp.size()) {
nuclear@0: 		return false;
nuclear@0: 	}
nuclear@0: 	cp.erase(cp.begin() + idx);
nuclear@12: 	inval_bounds();
nuclear@0: 	return true;
nuclear@0: }
nuclear@0: 
nuclear@12: void Curve::clear()
nuclear@0: {
nuclear@12: 	cp.clear();
nuclear@12: 	inval_bounds();
nuclear@0: }
nuclear@0: 
nuclear@2: bool Curve::empty() const
nuclear@2: {
nuclear@2: 	return cp.empty();
nuclear@2: }
nuclear@2: 
nuclear@2: int Curve::size() const
nuclear@0: {
nuclear@0: 	return (int)cp.size();
nuclear@0: }
nuclear@0: 
nuclear@12: Vector4 &Curve::operator [](int idx)
nuclear@12: {
nuclear@12: 	inval_bounds();
nuclear@12: 	return cp[idx];
nuclear@12: }
nuclear@12: 
nuclear@12: const Vector4 &Curve::operator [](int idx) const
nuclear@2: {
nuclear@2: 	return cp[idx];
nuclear@2: }
nuclear@2: 
nuclear@12: const Vector4 &Curve::get_point(int idx) const
nuclear@2: {
nuclear@2: 	return cp[idx];
nuclear@2: }
nuclear@2: 
nuclear@12: Vector3 Curve::get_point3(int idx) const
nuclear@0: {
nuclear@12: 	return Vector3(cp[idx].x, cp[idx].y, cp[idx].z);
nuclear@0: }
nuclear@0: 
nuclear@12: Vector2 Curve::get_point2(int idx) const
nuclear@0: {
nuclear@0: 	return Vector2(cp[idx].x, cp[idx].y);
nuclear@0: }
nuclear@0: 
nuclear@0: float Curve::get_weight(int idx) const
nuclear@0: {
nuclear@12: 	return cp[idx].w;
nuclear@12: }
nuclear@12: 
nuclear@12: bool Curve::set_point(int idx, const Vector3 &p, float weight)
nuclear@12: {
nuclear@12: 	if(idx < 0 || idx >= (int)cp.size()) {
nuclear@12: 		return false;
nuclear@12: 	}
nuclear@12: 	cp[idx] = Vector4(p.x, p.y, p.z, weight);
nuclear@12: 	inval_bounds();
nuclear@12: 	return true;
nuclear@0: }
nuclear@0: 
nuclear@0: bool Curve::set_point(int idx, const Vector2 &p, float weight)
nuclear@0: {
nuclear@0: 	if(idx < 0 || idx >= (int)cp.size()) {
nuclear@0: 		return false;
nuclear@0: 	}
nuclear@12: 	cp[idx] = Vector4(p.x, p.y, 0.0, weight);
nuclear@12: 	inval_bounds();
nuclear@0: 	return true;
nuclear@0: }
nuclear@0: 
nuclear@0: bool Curve::set_weight(int idx, float weight)
nuclear@0: {
nuclear@0: 	if(idx < 0 || idx >= (int)cp.size()) {
nuclear@0: 		return false;
nuclear@0: 	}
nuclear@12: 	cp[idx].w = weight;
nuclear@12: 	return true;
nuclear@12: }
nuclear@12: 
nuclear@12: bool Curve::move_point(int idx, const Vector3 &p)
nuclear@12: {
nuclear@12: 	if(idx < 0 || idx >= (int)cp.size()) {
nuclear@12: 		return false;
nuclear@12: 	}
nuclear@12: 	cp[idx] = Vector4(p.x, p.y, p.z, cp[idx].w);
nuclear@12: 	inval_bounds();
nuclear@0: 	return true;
nuclear@0: }
nuclear@0: 
nuclear@2: bool Curve::move_point(int idx, const Vector2 &p)
nuclear@2: {
nuclear@2: 	if(idx < 0 || idx >= (int)cp.size()) {
nuclear@2: 		return false;
nuclear@2: 	}
nuclear@12: 	cp[idx] = Vector4(p.x, p.y, 0.0f, cp[idx].w);
nuclear@12: 	inval_bounds();
nuclear@2: 	return true;
nuclear@2: }
nuclear@2: 
nuclear@12: 
nuclear@12: int Curve::nearest_point(const Vector3 &p) const
nuclear@0: {
nuclear@12: 	int res = -1;
nuclear@12: 	float bestsq = FLT_MAX;
nuclear@12: 
nuclear@12: 	for(size_t i=0; i<cp.size(); i++) {
nuclear@12: 		float d = (get_point3(i) - p).length_sq();
nuclear@12: 		if(d < bestsq) {
nuclear@12: 			bestsq = d;
nuclear@12: 			res = i;
nuclear@12: 		}
nuclear@12: 	}
nuclear@12: 	return res;
nuclear@12: }
nuclear@12: 
nuclear@12: int Curve::nearest_point(const Vector2 &p) const
nuclear@12: {
nuclear@12: 	int res = -1;
nuclear@12: 	float bestsq = FLT_MAX;
nuclear@12: 
nuclear@12: 	for(size_t i=0; i<cp.size(); i++) {
nuclear@12: 		float d = (get_point2(i) - p).length_sq();
nuclear@12: 		if(d < bestsq) {
nuclear@12: 			bestsq = d;
nuclear@12: 			res = i;
nuclear@12: 		}
nuclear@12: 	}
nuclear@12: 	return res;
nuclear@12: }
nuclear@12: 
nuclear@12: 
nuclear@12: void Curve::inval_bounds() const
nuclear@12: {
nuclear@12: 	bbvalid = false;
nuclear@12: }
nuclear@12: 
nuclear@12: void Curve::calc_bounds() const
nuclear@12: {
nuclear@12: 	calc_bbox(&bbmin, &bbmax);
nuclear@12: 	bbvalid = true;
nuclear@12: }
nuclear@12: 
nuclear@12: void Curve::get_bbox(Vector3 *bbmin, Vector3 *bbmax) const
nuclear@12: {
nuclear@12: 	if(!bbvalid) {
nuclear@12: 		calc_bounds();
nuclear@12: 	}
nuclear@12: 	*bbmin = this->bbmin;
nuclear@12: 	*bbmax = this->bbmax;
nuclear@12: }
nuclear@12: 
nuclear@12: void Curve::calc_bbox(Vector3 *bbmin, Vector3 *bbmax) const
nuclear@12: {
nuclear@12: 	if(empty()) {
nuclear@12: 		*bbmin = *bbmax = Vector3(0, 0, 0);
nuclear@12: 		return;
nuclear@12: 	}
nuclear@12: 
nuclear@12: 	Vector3 bmin = cp[0];
nuclear@12: 	Vector3 bmax = bmin;
nuclear@12: 	for(size_t i=1; i<cp.size(); i++) {
nuclear@12: 		const Vector4 &v = cp[i];
nuclear@12: 		for(int j=0; j<3; j++) {
nuclear@12: 			if(v[j] < bmin[j]) bmin[j] = v[j];
nuclear@12: 			if(v[j] > bmax[j]) bmax[j] = v[j];
nuclear@12: 		}
nuclear@12: 	}
nuclear@12: 	*bbmin = bmin;
nuclear@12: 	*bbmax = bmax;
nuclear@12: }
nuclear@12: 
nuclear@12: void Curve::normalize()
nuclear@12: {
nuclear@12: 	if(!bbvalid) {
nuclear@12: 		calc_bounds();
nuclear@12: 	}
nuclear@12: 
nuclear@12: 	Vector3 bsize = bbmax - bbmin;
nuclear@12: 	Vector3 boffs = (bbmin + bbmax) * 0.5;
nuclear@12: 
nuclear@12: 	Vector3 bscale;
nuclear@12: 	bscale.x = bsize.x == 0.0f ? 1.0f : 1.0f / bsize.x;
nuclear@12: 	bscale.y = bsize.y == 0.0f ? 1.0f : 1.0f / bsize.y;
nuclear@12: 	bscale.z = bsize.z == 0.0f ? 1.0f : 1.0f / bsize.z;
nuclear@12: 
nuclear@12: 	for(size_t i=0; i<cp.size(); i++) {
nuclear@12: 		cp[i].x = (cp[i].x - boffs.x) * bscale.x;
nuclear@12: 		cp[i].y = (cp[i].y - boffs.y) * bscale.y;
nuclear@12: 		cp[i].z = (cp[i].z - boffs.z) * bscale.z;
nuclear@12: 	}
nuclear@12: 	inval_bounds();
nuclear@12: }
nuclear@12: 
nuclear@13: Vector3 Curve::proj_point(const Vector3 &p, float refine_thres) const
nuclear@12: {
nuclear@13: 	// first step through the curve a few times and find the nearest of them
nuclear@12: 	int num_cp = size();
nuclear@13: 	int num_steps = num_cp * 5;	// arbitrary number; sounds ok
nuclear@13: 	float dt = 1.0f / (float)(num_steps - 1);
nuclear@12: 
nuclear@13: 	float best_distsq = FLT_MAX;
nuclear@13: 	float best_t = 0.0f;
nuclear@13: 	Vector3 best_pp;
nuclear@12: 
nuclear@13: 	float t = 0.0f;
nuclear@13: 	for(int i=0; i<num_steps; i++) {
nuclear@13: 		Vector3 pp = interpolate(t);
nuclear@13: 		float distsq = (pp - p).length_sq();
nuclear@13: 		if(distsq < best_distsq) {
nuclear@13: 			best_distsq = distsq;
nuclear@13: 			best_pp = pp;
nuclear@13: 			best_t = t;
nuclear@13: 		}
nuclear@13: 		t += dt;
nuclear@13: 	}
nuclear@12: 
nuclear@13: 	// refine by gradient descent
nuclear@13: 	float dist = best_distsq;
nuclear@13: 	t = best_t;
nuclear@13: 	dt *= 0.05;
nuclear@13: 	for(;;) {
nuclear@13: 		float tn = t + dt;
nuclear@13: 		float tp = t - dt;
nuclear@12: 
nuclear@13: 		float dn = (interpolate(tn) - p).length_sq();
nuclear@13: 		float dp = (interpolate(tp) - p).length_sq();
nuclear@12: 
nuclear@13: 		if(fabs(dn - dp) < refine_thres * refine_thres) {
nuclear@12: 			break;
nuclear@12: 		}
nuclear@12: 
nuclear@13: 		if(dn < dist) {
nuclear@13: 			t = tn;
nuclear@13: 			dist = dn;
nuclear@13: 		} else if(dp < dist) {
nuclear@13: 			t = tp;
nuclear@13: 			dist = dp;
nuclear@12: 		} else {
nuclear@13: 			break;	// found the minimum
nuclear@12: 		}
nuclear@12: 	}
nuclear@13: 
nuclear@13: 	return interpolate(t);
nuclear@12: }
nuclear@12: 
nuclear@13: float Curve::distance(const Vector3 &p) const
nuclear@13: {
nuclear@13: 	return (proj_point(p) - p).length();
nuclear@13: }
nuclear@13: 
nuclear@13: float Curve::distance_sq(const Vector3 &p) const
nuclear@13: {
nuclear@13: 	return fabs((proj_point(p) - p).length_sq());
nuclear@13: }
nuclear@13: 
nuclear@13: 
nuclear@12: Vector3 Curve::interpolate_segment(int a, int b, float t) const
nuclear@12: {
nuclear@12: 	int num_cp = size();
nuclear@12: 
nuclear@12: 	if(t < 0.0) t = 0.0;
nuclear@12: 	if(t > 1.0) t = 1.0;
nuclear@12: 
nuclear@12: 	Vector4 res;
nuclear@12: 	if(type == CURVE_LINEAR || num_cp == 2) {
nuclear@12: 		res = lerp(cp[a], cp[b], t);
nuclear@12: 	} else {
nuclear@12: 		int prev = a <= 0 ? a : a - 1;
nuclear@12: 		int next = b >= num_cp - 1 ? b : b + 1;
nuclear@12: 
nuclear@12: 		if(type == CURVE_HERMITE) {
nuclear@12: 			res = catmull_rom_spline(cp[prev], cp[a], cp[b], cp[next], t);
nuclear@12: 		} else {
nuclear@12: 			res = bspline(cp[prev], cp[a], cp[b], cp[next], t);
nuclear@12: 			if(res.w != 0.0f) {
nuclear@12: 				res.x /= res.w;
nuclear@12: 				res.y /= res.w;
nuclear@13: 				res.z /= res.w;
nuclear@12: 			}
nuclear@12: 		}
nuclear@12: 	}
nuclear@12: 
nuclear@12: 	return Vector3(res.x, res.y, res.z);
nuclear@12: }
nuclear@12: 
nuclear@12: Vector3 Curve::interpolate(float t) const
nuclear@12: {
nuclear@12: 	if(empty()) {
nuclear@12: 		return Vector3(0, 0, 0);
nuclear@0: 	}
nuclear@2: 
nuclear@2: 	int num_cp = (int)cp.size();
nuclear@0: 	if(num_cp == 1) {
nuclear@12: 		return Vector3(cp[0].x, cp[0].y, cp[0].z);
nuclear@0: 	}
nuclear@0: 
nuclear@13: 	if(t < 0.0) t = 0.0;
nuclear@13: 	if(t > 1.0) t = 1.0;
nuclear@13: 
nuclear@2: 	int idx0 = std::min((int)floor(t * (num_cp - 1)), num_cp - 2);
nuclear@0: 	int idx1 = idx0 + 1;
nuclear@0: 
nuclear@0: 	float dt = 1.0 / (float)(num_cp - 1);
nuclear@0: 	float t0 = (float)idx0 * dt;
nuclear@0: 	float t1 = (float)idx1 * dt;
nuclear@0: 
nuclear@0: 	t = (t - t0) / (t1 - t0);
nuclear@0: 
nuclear@12: 	return interpolate_segment(idx0, idx1, t);
nuclear@12: }
nuclear@0: 
nuclear@12: Vector2 Curve::interpolate2(float t) const
nuclear@12: {
nuclear@12: 	Vector3 res = interpolate(t);
nuclear@0: 	return Vector2(res.x, res.y);
nuclear@0: }
nuclear@0: 
nuclear@12: Vector3 Curve::operator ()(float t) const
nuclear@0: {
nuclear@0: 	return interpolate(t);
nuclear@0: }