Merge pull request #155 from danjpar/master

Added curves.go

math32/curves.go

@@ -0,0 +1,205 @@
+// Copyright 2016 The G3N Authors. All rights reserved.
+// Use of this source code is governed by a BSD-style
+// license that can be found in the LICENSE file.
+
+package math32
+
+// Curve constructs an array of Vector3
+type Curve struct {
+	points []Vector3
+	length float32
+}
+
+func (c *Curve) GetPoints() []Vector3 {
+	return c.points
+}
+
+func (c *Curve) GetLength() float32 {
+	return c.length
+}
+
+func (c *Curve) SetLength() {
+	points := c.points
+	l := float32(0.0)
+	for i := 1; i < len(points); i++ {
+		p0 := points[i].Clone()
+		p1 := points[i - 1].Clone()
+		l += (p0.Sub(p1)).Length()
+	}
+	c.length = l
+}
+
+// Continue combines two curves
+// creates and returns a pointer to a new curve
+// combined curves are unaffected
+func (c *Curve) Continue(other *Curve) *Curve {
+	last := c.points[len(c.points) - 1].Clone()
+	first := other.points[0].Clone()
+
+	var continued, otherpoints []Vector3
+	for i := 0; i < len(c.points); i++ {
+		continued = append(continued, *c.points[i].Clone())
+	}
+	for i := 1; i < len(other.points); i++ {
+		otherpoints = append(otherpoints, *other.points[i].Clone())
+	}
+	for i := 0; i < len(otherpoints); i++ {
+		continued = append(continued, *otherpoints[i].Sub(first).Add(last))
+	}
+	newC := new(Curve)
+	newC.points = continued
+	newC.SetLength()
+	return newC
+}
+
+// NewBezierQuadratic creates and returns a pointer to a new curve
+// Uses Vector3 pointers origin, control, and destination to calculate with
+// int npoints as the desired number of points along the curve
+func NewBezierQuadratic(origin, control, destination *Vector3, npoints int) *Curve {
+	c := new(Curve)
+
+	if npoints <= 2 {
+		npoints = 3
+	}
+	var equation func(float32, float32, float32, float32) float32
+	equation = func(t, v0, v1, v2 float32) float32 {
+		a0 := 1.0 - t
+		result := a0 * a0 * v0 + 2.0 * t * a0 * v1 + t * t * v2
+		return result
+	}
+	var bezier []Vector3
+
+	for i := 0; i <= npoints; i++ {
+		t := float32(i) / float32(npoints)
+		x := equation(t, origin.X, control.X, destination.X)
+		y := equation(t, origin.Y, control.Y, destination.Y)
+		z := equation(t, origin.Z, control.Z, destination.Z)
+		vect := NewVector3(x, y, z)
+		bezier = append(bezier, *vect)
+	}
+
+	c.points = bezier
+	c.SetLength()
+	return c
+}
+
+// NewBezierCubic creates and returns a pointer to a new curve
+// Uses Vector3 pointers origin, control1, control2, and destination to calculate with
+// int npoints as the desired number of points along the curve
+func NewBezierCubic(origin, control1, control2, destination *Vector3, npoints int) *Curve {
+	c := new(Curve)
+
+	if npoints <= 3 {
+		npoints = 4
+	}
+	
+	var equation func(float32, float32, float32, float32, float32) float32
+	equation = func(t, v0, v1, v2, v3 float32) float32 {
+		a0 := 1.0 - t
+		result := a0 * a0 * a0 * v0 + 3.0 * t * a0 * a0 * v1 + 3.0 * t * t * a0 * v2 + t * t * t * v3
+		return result
+	}
+	var bezier []Vector3
+
+	for i := 0; i <= npoints; i++ {
+		t := float32(i) / float32(npoints)
+		x := equation(t, origin.X, control1.X, control2.X, destination.X)
+		y := equation(t, origin.Y, control1.Y, control2.Y, destination.Y)
+		z := equation(t, origin.Z, control1.Z, control2.Z, destination.Z)
+		vect := NewVector3(x, y, z)
+		bezier = append(bezier, *vect)
+	}
+
+	c.points = bezier
+	c.SetLength()
+	return c
+}
+
+// NewHermiteSpline creates and returns a pointer to a new curve
+// Uses Vector3 pointers origin, tangent1, destination, and tangent2 to calculate with
+// int npoints as the desired number of points along the curve
+func NewHermiteSpline(origin, tangent1, destination, tangent2 *Vector3, npoints int) *Curve {
+	c := new(Curve)
+
+	var equation func(float32, *Vector3, *Vector3, *Vector3, *Vector3) *Vector3
+	equation = func(t float32, v0, tan0, v1, tan1 *Vector3) *Vector3 {
+		t2 := t * t
+		t3 := t * t2
+		p0 := (2.0 * t3) - (3.0 * t2) + 1.0
+		p1 := (-2.0 * t3) + (3.0 * t2)
+		p2 := t3 - (2.0 * t2) + t
+		p3 := t3 - t2
+		x := (v0.X * p0) + (v1.X * p1) + (tan0.X * p2) + (tan1.X * p3)
+		y := (v0.Y * p0) + (v1.Y * p1) + (tan0.Y * p2) + (tan1.Y * p3)
+		z := (v0.Z * p0) + (v1.Z * p1) + (tan0.Z * p2) + (tan1.Z * p3)
+		return NewVector3(x, y, z)
+	}
+
+	step := float32(1.0) / float32(npoints)
+	var hermite []Vector3
+	for i := 0; i <= npoints; i++ {
+		vect := equation(float32(i) * step, origin, tangent1, destination, tangent2)
+		hermite = append(hermite, *vect)
+	}
+	c.points = hermite
+	c.SetLength()
+	return c
+}
+
+// NewCatmullRomSpline creates and returns a pointer to a new curve
+// Uses array of Vector3 pointers with int npoints as the desired number of points between supplied points
+// Use Boolean closed with true to close the start and end points
+func NewCatmullRomSpline(points []*Vector3, npoints int, closed bool) *Curve {
+	c := new(Curve)
+
+	var equation func(float32, *Vector3, *Vector3, *Vector3, *Vector3) *Vector3
+	equation = func(t float32, v0, v1, v2, v3 *Vector3) *Vector3 {
+		t2 := t * t;
+		t3 := t * t2;
+		x := 0.5 * ((((2.0 * v1.X) + ((-v0.X + v2.X) * t)) +
+			(((((2.0 * v0.X) - (5.0 * v1.X)) + (4.0 * v2.X)) - v3.X) * t2)) +
+			((((-v0.X + (3.0 * v1.X)) - (3.0 * v2.X)) + v3.X) * t3));
+		y := 0.5 * ((((2.0 * v1.Y) + ((-v0.Y + v2.Y) * t)) +
+			(((((2.0 * v0.Y) - (5.0 * v1.Y)) + (4.0 * v2.Y)) - v3.Y) * t2)) +
+			((((-v0.Y + (3.0 * v1.Y)) - (3.0 * v2.Y)) + v3.Y) * t3));
+		z := 0.5 * ((((2.0 * v1.Z) + ((-v0.Z + v2.Z) * t)) +
+			(((((2.0 * v0.Z) - (5.0 * v1.Z)) + (4.0 * v2.Z)) - v3.Z) * t2)) +
+			((((-v0.Z + (3.0 * v1.Z)) - (3.0 * v2.Z)) + v3.Z) * t3));
+		return NewVector3(x, y, z);
+	}
+
+	step := float32(1.0) / float32(npoints)
+	var catmull []Vector3
+	var t float32
+	if closed {
+		count := len(points)
+		for i := 0; i < count; i++ {
+			t = 0.0
+			for n := 0; n < npoints; n++ {
+				vect := equation(t, points[i % count], points[(i + 1) % count], points[(i + 2) % count], points[(i + 3) % count])
+				catmull = append(catmull, *vect)
+				t += step
+			}
+		}
+		catmull = append(catmull, catmull[0])
+	} else {
+		total := []*Vector3{points[0].Clone()}
+		total = append(total, points...)
+		total = append(total, points[len(points) - 1].Clone())
+		var i int
+		for i = 0; i < len(total) - 3; i++ {
+			t = 0
+			for n := 0; n < npoints; n++ {
+				vect := equation(t, total[i], total[i + 1], total[i + 2], total[i + 3])
+				catmull = append(catmull, *vect)
+				t += step
+			}
+		}
+		i--
+		vect := equation(t, total[i], total[i + 1], total[i + 2], total[i + 3])
+		catmull = append(catmull, *vect)
+	}
+	c.points = catmull
+	c.SetLength()
+	return c
+}