From 05f7fe0afcd8a3ecdbb11575492b95cadb6ef769 Mon Sep 17 00:00:00 2001 From: mseddon Date: Thu, 5 Nov 2015 11:37:15 +0000 Subject: [PATCH] Verified identities and implementations. Moved to column order --- .gitignore | 2 + README.md | 143 +- build.sbt | 14 +- .../test/scala/scryetek/vecmath/Vec2.scala | 34 - project/plugins.sbt | 4 +- .../scala/scryetek/vecmath/AngleAxis.scala | 7 + .../main/scala/scryetek/vecmath/Mat2.scala | 283 ++-- .../main/scala/scryetek/vecmath/Mat2d.scala | 331 +++-- .../main/scala/scryetek/vecmath/Mat3.scala | 774 +++++++--- .../main/scala/scryetek/vecmath/Mat4.scala | 1314 +++++++++++------ .../main/scala/scryetek/vecmath/Quat.scala | 438 +++--- .../main/scala/scryetek/vecmath/Vec2.scala | 257 +++- .../main/scala/scryetek/vecmath/Vec3.scala | 277 +++- .../main/scala/scryetek/vecmath/Vec4.scala | 220 ++- .../scryetek/vecmath/MatSpecification.scala | 403 +++++ .../scryetek/vecmath/QuatSpecification.scala | 68 + .../scryetek/vecmath/VecSpecification.scala | 286 ++++ .../test/scala/scryetek/vecmath/package.scala | 93 ++ 18 files changed, 3642 insertions(+), 1306 deletions(-) delete mode 100644 jvm/src/test/scala/scryetek/vecmath/Vec2.scala create mode 100644 shared/src/main/scala/scryetek/vecmath/AngleAxis.scala create mode 100644 shared/src/test/scala/scryetek/vecmath/MatSpecification.scala create mode 100644 shared/src/test/scala/scryetek/vecmath/QuatSpecification.scala create mode 100644 shared/src/test/scala/scryetek/vecmath/VecSpecification.scala create mode 100644 shared/src/test/scala/scryetek/vecmath/package.scala diff --git a/.gitignore b/.gitignore index 111f416..afe7483 100644 --- a/.gitignore +++ b/.gitignore @@ -5,6 +5,8 @@ /jvm/target /js/target +*.sc +TODO.* ### Emacs template # -*- mode: gitignore; -*- diff --git a/README.md b/README.md index 8861e7a..01b0f32 100644 --- a/README.md +++ b/README.md @@ -7,13 +7,150 @@ A simple vector maths library for graphics programming. Add the following to your build.sbt: ```scala -libraryDependencies += "com.scryetek" %% "vecmath" % "0.1" +libraryDependencies += "com.scryetek" %% "vecmath" % "0.3.1" ``` Or if you're using ScalaJS ```scala -libraryDependencies += "com.scryetek" %%% "vecmath" % "0.1" +libraryDependencies += "com.scryetek" %%% "vecmath" % "0.3.1" ``` -Proper documentation to follow shortly \ No newline at end of file +Proper documentation to follow shortly + +### Scope + +Currently, `vecmath` supports common operations on the following classes of matrices of Floats: + +* 4x4 matrix (`Mat4`) +* 3x3 matrix (`Mat3`) +* 2x2 matrix (`Mat2`) +* 2x3 matrix (`Mat2d`) - a 3x3 matrix with a fixed bottom row of `0 0 1` + +The following vector sizes are supported + +`Vec4`, `Vec3`, `Vec2` + +And of course, no graphics library would be complete without Quaternions, in the `Quat` package. + +Matrices are stored in *column major* order- that is, how you would write them. Angles are always assumed to be +given in radians. A 4x4 matrix (`Mat4`) has the following positions: + +``` +m00 m01 m02 m03 +m10 m11 m12 m13 +m20 m21 m22 m23 +m30 m31 m32 m33 +``` + +Quaternions are defined with imaginary components `x`, `y`, `z`, and real component `w`, in that order. + +Every class supports a `.copy()` method, which works like a case class copy method (but these classes are not case +classes, since they contain mutable members). + +Every class also supports an analogous `.set()` command, with two variants. The first variant has the same signature +as `.copy()`, and destructively re-assigns fields, and the second takes a single parameter of the same type, and +sets the object to an exact copy of the parameter. + +## Mutable vs Immutable usage. + +There are usually two ways of doing things (particularly for common operations) with the vecmath library. You can use +the pleasant to use and read operator way, which I'd recommend you stick with until it's too slow: + +```scala +val v = Vec2(1,2) + Vec2(3,4) * 5 +``` + +Or you can use the hairier but potentially far more efficient mutable way: + +```scala +val v2 = Vec2(1,2) +v2.add(Vec2(3,4)) // or even v2.add(3,4) +// v2 is now Vec(4,6) +``` + +Most mutating methods take an optional 'out' parameter, which stores the result in another object entirely. + +```scala +val v2 = Vec2(1,2) +val out = Vec2() +v2.add(Vec2(3,4), out) // or v2.add(3,4, out) +// out is now Vec(4,6), but v2 is unchanged +``` + +Nearly all methods are marked `@inline`, but since neither `scalac` nor `proguard` dare inline constructors, we sadly +can't expect to get purely stack allocated code like you might in C++. However- inlining using the mutable +methods does work, if you care to compile with `-optimise`. + +#### Common Sense Alert + +You must look after what is mutable and what is not yourself, don't go mutating other people's Vec2's, and for heaven's +sake don't place vectors as keys in hashtables and then mutate them unless you like losing them forever. For less pain, +create a defensive copy of the object you want to mangle using the `.copy()` method. + +### Matrix Operations + +Assume we have `Mat4` matrices, M1 and M2. We can say: + +```scala +M3 = M1 * M2 +``` + +This is equivalent to: + +```scala +M1.postMultiply(M2, M3) +``` + +The opposite direction is also supported: + +```scala +M3 = M2 * M1 +``` + +Is the same as: + +```scala +M1.preMultiply(M2, M3) +``` + +Note sometimes we have verb and adjective variations of the same method. + +```scala +M1.invert(M2) +``` + +The verb form is mutating, while the adjective creates a copy: + +```scala +M2 = M1.inverted +``` + +## Documentation + +See the [ScalaDoc](http://mseddon.github.io/vecmath/api/#scryetek.vecmath.package) + +### Test coverage/correctness + +The methods employed in this library are verified using Mathematica 10, (primarily to quickly ensure implementations +conform to the same conventions etc, since there is some variation) and nearly every method is property tested +using the excellent [Scalacheck](https://www.scalacheck.org/) based on the derived laws, and standard definitions. + +Speed is always considered over precision, and numerically this library is not overly stable. + +Occasionally we get unlucky and numerically a test fails once in a while. This is not considered a huge loss unless +the error is way above expectations. Each property is tested 5000 times, though the generators could probably be +made more evil, particularly with respect to triggering singularities. + +### Future Plans + +Currently, this library is simply a 'vector math library', whatever that means. It includes enough linear algebra to +get things done conveniently with WebGL for example. In future I will extend it to support raytracing primitives, +spatial partitioning and probably things like triangulation. At which point, maybe 'vecmath' will be a fairly bad name. + +Such is life. + +### Acknowledgements + +This library contains implementations based on the excellent [Euclidean Space](http://www.euclideanspace.com/) site, +and probably more things from the [Graphics Gems](http://www.graphicsgems.org) series than I realise. diff --git a/build.sbt b/build.sbt index 5a0cbe9..279ce4e 100644 --- a/build.sbt +++ b/build.sbt @@ -1,4 +1,6 @@ import sbt.Keys._ +import sbt.Tests + lazy val root = project.in(file(".")). aggregate(vecMathJS, vecMathJVM). @@ -7,13 +9,17 @@ lazy val root = project.in(file(".")). publishLocal := {} ) -lazy val vecMath = crossProject.in(file(".")). - settings( - scalaVersion := "2.11.6", +lazy val vecMath = crossProject.in(file(".")).settings( + crossScalaVersions := Seq("2.10.6", "2.11.7"), + scalaVersion := "2.11.7", organization := "com.scryetek", name := "vecmath", - version := "0.2-SNAPSHOT", + version := "0.3.1", description := "A simple vector maths library for graphics programming.", + + libraryDependencies += "org.scalacheck" %% "scalacheck" % "1.12.5" % "test", + testOptions in Test += Tests.Argument(TestFrameworks.ScalaCheck, "-minSuccessfulTests", "5000"), + homepage := Some(url("https://github.org/mseddon/vecmath")), scmInfo := Some(ScmInfo( url("https://github.com/mseddon/vecmath"), diff --git a/jvm/src/test/scala/scryetek/vecmath/Vec2.scala b/jvm/src/test/scala/scryetek/vecmath/Vec2.scala deleted file mode 100644 index 381b85e..0000000 --- a/jvm/src/test/scala/scryetek/vecmath/Vec2.scala +++ /dev/null @@ -1,34 +0,0 @@ -package scryetek.vecmath - -import org.scalatest._ - -/** - * Created by matt on 08/06/15. - */ -class MatSpec extends FlatSpec with Matchers { - val Epsilon = 0.001f - - "A Vec2" should "be correctly transformed by a translation Mat2d" in { - val v = Vec2(0,0) - (Mat2d().translate(Vec2(3, 4)) * v - Vec2(3, 4)).magnitude should be < Epsilon - } - - "A Vec2" should "be correctly transformed by a translation Mat3" in { - val v = Vec2(0,0) - (Mat3().translate(Vec2(3, 4)) * v - Vec2(3, 4)).magnitude should be < Epsilon - } - - "A translation Mat2d" should "be invertable" in { - val v = Vec2(0,0) - val m = Mat2d().translate(Vec2(3, 4)) - - (m * (m.invertInto(Mat2d()) * v - Vec2(0,0))).magnitude should be < Epsilon - } - - "A rotation Mat2d" should "be invertable" in { - val v = Vec2(0,0) - val m = Mat2d().rotate(Math.PI.toFloat*2) - - (m * (m.invertInto(Mat2d()) * v - Vec2(0,0))).magnitude should be < Epsilon - } -} diff --git a/project/plugins.sbt b/project/plugins.sbt index 45442bf..65b6f2b 100644 --- a/project/plugins.sbt +++ b/project/plugins.sbt @@ -1,5 +1,7 @@ logLevel := Level.Warn -addSbtPlugin("org.scala-js" % "sbt-scalajs" % "0.6.3") +addSbtPlugin("org.scala-js" % "sbt-scalajs" % "0.6.5") addSbtPlugin("com.jsuereth" % "sbt-pgp" % "1.0.0") + +addSbtPlugin("org.scoverage" % "sbt-scoverage" % "1.3.3") diff --git a/shared/src/main/scala/scryetek/vecmath/AngleAxis.scala b/shared/src/main/scala/scryetek/vecmath/AngleAxis.scala new file mode 100644 index 0000000..7680ea0 --- /dev/null +++ b/shared/src/main/scala/scryetek/vecmath/AngleAxis.scala @@ -0,0 +1,7 @@ +package scryetek.vecmath + +@inline +case class AngleAxis(angle: Float, axis: Vec3) { + @inline + def toQuat = Quat.fromAngleAxis(angle, axis.x, axis.y, axis.z) +} diff --git a/shared/src/main/scala/scryetek/vecmath/Mat2.scala b/shared/src/main/scala/scryetek/vecmath/Mat2.scala index 40c80cd..be07dae 100644 --- a/shared/src/main/scala/scryetek/vecmath/Mat2.scala +++ b/shared/src/main/scala/scryetek/vecmath/Mat2.scala @@ -1,107 +1,178 @@ -package scryetek.vecmath - -/** - * Created by Matt on 06/07/2014. - */ -case class Mat2(var m00: Float, var m01: Float, - var m10: Float, var m11: Float) { - def this() = - this(1, 0, - 0, 1) - - def this(m: Mat2) = - this(m.m00, m.m01, - m.m10, m.m11) - - def set(m00: Float, m01: Float, - m10: Float, m11: Float): Mat2 = { - this.m00 = m00; this.m01 = m01 - this.m10 = m10; this.m11 = m11 - this - } - - def set(m: Mat2): Mat2 = { - this.m00 = m.m00; this.m01 = m.m01 - this.m10 = m.m10; this.m11 = m.m11 - this - } - - def setIdentity: Mat2 = { - m00 = 1; m01 = 0 - m10 = 0; m11 = 1 - this - } - - def transposeInto(m: Mat2): Mat2 = - m.set(m00, m10, - m01, m11) - - def transpose: Mat2 = - transposeInto(this) - - def invertInto(m: Mat2): Mat2 = { - var det = m00*m11 - m10*m01 - - if(det == 0) - return null - - det = 1/det - - m.set( m11*det, -m01*det, - -m10*det, m00*det) - } - - def invert: Mat2 = - invertInto(this) - - def adjointInto(m: Mat2): Mat2 = - m.set( m11, -m01, - -m10, m00) - - def adjoint: Mat2 = - adjointInto(this) - - def determinant: Float = m00*m11 - m10*m01 - - def multiplyInto(out: Mat2, m: Mat2): Mat2 = - out.set(m00*m.m00 + m10*m.m01, - m01*m.m00 + m11*m.m01, - m00*m.m10 + m10*m.m11, - m01*m.m10 + m11*m.m11) - - def multiply(m: Mat2): Mat2 = - multiplyInto(this, m) - - def rotateInto(out: Mat2, angle: Float): Mat2 = { - val c = Math.cos(angle).toFloat - val s = Math.sin(angle).toFloat - out.set(m00 * c + m10 * s, m01 * c + m11 * s, - m00 * -s + m10 * c, m01 * -s + m11 * c) - } - - def rotate(angle: Float): Mat2 = - rotateInto(this, angle) - - def scaleInto(out: Mat2, v: Vec2): Mat2 = - out.set(m00*v.x, m01*v.x, - m10*v.y, m11*v.y) - - def scale(v: Vec2): Mat2 = - scaleInto(this, v) - - override def toString: String = - s"Mat2($m00, $m01,\n" + - s" $m10, $m11)" - - def toArray(array: Array[Float]): Array[Float] = { - array(0) = m00; array(1) = m01 - array(2) = m10; array(3) = m11 - array - } -} - -object Mat2 { - def apply(): Mat2 = new Mat2() - - def apply(m: Mat2): Mat2 = new Mat2(m) +package scryetek.vecmath + +final class Mat2( + var m00: Float, var m01: Float, + var m10: Float, var m11: Float) { + @inline + def set( + m00: Float = this.m00, m01: Float = this.m01, + m10: Float = this.m10, m11: Float = this.m11): Mat2 = { + this.m00 = m00; this.m01 = m01 + this.m10 = m10; this.m11 = m11 + this + } + + @inline + def set(m: Mat2): Mat2 = + set(m.m00, m.m01, + m.m10, m.m11) + + @inline + def this() = this(1, 0, 0, 1) + + @inline + def determinant = m00*m11-m01*m10 + + @inline + def inverted = { + var det = determinant + assert(det != 0, "Matrix is not invertable") + det = 1/det + Mat2( m11*det, -m01*det, + -m10*det, m00*det) + } + + @inline + def invert(out: Mat2): Mat2 = { + var det = determinant + assert(det != 0, "Matrix is not invertable") + det = 1/det + out.set( m11*det, -m01*det, + -m10*det, m00*det) + } + + /** Returns the transpose of this matrix. */ + @inline + def transposed = + Mat2(m00, m10, + m01, m11) + + /** Transposes this matrix into the specified out matrix. */ + @inline + def transpose(out: Mat2 = this): Mat2 = + out.set(m00, m10, + m01, m11) + + /** Returns the adjoint of this matrix. */ + @inline + def adjointed = + Mat2( m11, -m01, + -m10, m00) + + /** Puts the adjoint of this matrix into the specified out matrix. */ + @inline + def adjoint(out: Mat2): Mat2 = + out.set( m11, -m01, + -m10, m00) + + + /** Adds two matrices */ + @inline + def +(m: Mat2): Mat2 = + Mat2(m00 + m.m00, m01 + m.m01, + m10 + m.m10, m11 + m.m11) + + /** Destructively add another matrix to this matrix into the output matrix. */ + @inline + def add(m: Mat2, out: Mat2 = this): Mat2 = + out.set(m00 + m.m00, m01 + m.m01, + m10 + m.m10, m11 + m.m11) + + + /** Adds two matrices */ + @inline + def -(m: Mat2): Mat2 = + Mat2(m00 - m.m00, m01 - m.m01, + m10 - m.m10, m11 - m.m11) + + /** Destructively add another matrix to this matrix into the output matrix. */ + @inline + def sub(m: Mat2, out: Mat2 = this): Mat2 = + out.set(m00 - m.m00, m01 - m.m01, + m10 - m.m10, m11 - m.m11) + + /** Transforms a 2-vector by this matrix. */ + @inline + def *(v: Vec2): Vec2 = + Vec2(m00*v.x + m01*v.y, m10*v.x + m11*v.y) + + /** Transforms a 2-vector into the specified out vector. */ + @inline + def mul(v: Vec2, out: Vec2): Vec2 = + out.set(m00*v.x + m01*v.y, m10*v.x + m11*v.y) + + /** Destructively transforms a 2-vector. */ + @inline + def mul(v: Vec2): Vec2 = + mul(v,v) + + /** Returns a scaled copy of this matrix. */ + @inline + def *(s: Float): Mat2 = + Mat2(m00*s, m01*s, + m10*s, m11*s) + + @inline + def scale(s: Float, out: Mat2 = this): Mat2 = + out.set(m00*s, m01*s, + m10*s, m11*s) + + /** Returns the result of multiplying this matrix by another matrix. */ + @inline + def *(m: Mat2): Mat2 = + Mat2(m00*m.m00 + m01*m.m10, m00*m.m01 + m01*m.m11, + m10*m.m00 + m11*m.m10, m10*m.m01 + m11*m.m11) + + /** Copies the result of postmultiplying another matrix by this matrix into the specified output matrix. */ + @inline + def postMultiply(m: Mat2, out: Mat2 = this): Mat2 = + out.set(m00*m.m00 + m01*m.m10, m00*m.m01 + m01*m.m11, + m10*m.m00 + m11*m.m10, m10*m.m01 + m11*m.m11) + + /** Copies the result of premultiplying another matrix by this matrix into the specified output matrix. */ + @inline + def preMultiply(m: Mat2, out: Mat2 = this): Mat2 = + out.set(m.m00*m00 + m.m01*m10, m.m00*m01 + m.m01*m11, + m.m10*m00 + m.m11*m10, m.m10*m01 + m.m11*m11) + + def copy(m00: Float = m00, m01: Float = m01, + m10: Float = m10, m11: Float = m11) = + Mat2(m00, m01, + m10, m11) + + override def toString = + s"Mat2(${m00}f,${m01}f,${m10}f,${m11}f)" + + override def equals(o: Any): Boolean = o match { + case v: Mat2 => m00 == v.m00 && m01 == v.m01 && m10 == v.m10 && m11 == v.m11 + case _ => false + } + + override def hashCode: Int = + m00.hashCode() * 19 + m01.hashCode() * 23 + + m10.hashCode() * 31 + m11.hashCode() * 37 +} + +object Mat2 { + def apply() = new Mat2() + + def apply(m00: Float, m01: Float, m10: Float, m11: Float) = + new Mat2(m00, m01, m10, m11) + + @inline + def rotate(angle: Float): Mat2 = { + val c = math.cos(angle).toFloat + val s = math.sin(angle).toFloat + Mat2(c, -s, + s, c) + } + + @inline + def scale(x: Float, y: Float): Mat2 = + Mat2(x, 0, + 0, y) + + @inline + def scale(s: Vec2): Mat2 = + scale(s.x, s.y) } \ No newline at end of file diff --git a/shared/src/main/scala/scryetek/vecmath/Mat2d.scala b/shared/src/main/scala/scryetek/vecmath/Mat2d.scala index 1a3a2c9..fb0db12 100644 --- a/shared/src/main/scala/scryetek/vecmath/Mat2d.scala +++ b/shared/src/main/scala/scryetek/vecmath/Mat2d.scala @@ -1,141 +1,192 @@ -package scryetek.vecmath - -/** - * A 2x3 matrix for affine transforms in R2 - */ -class Mat2d(var m0: Float, var m1: Float, - var m2: Float, var m3: Float, - var m4: Float, var m5: Float) { - - /** Construct a new Mat2d initialized to the identity */ - def this() = this(1, 0, - 0, 1, - 0, 0) - - /** Construct a copy of a Mat2d. */ - def this(m: Mat2d) = - this(m.m0, m.m1, - m.m2, m.m3, - m.m4, m.m5) - - /** Construct a copy of a Mat3 with the rightmost column removed */ - def this(m: Mat3) = - this(m.m00, m.m01, - m.m10, m.m11, - m.m20, m.m21) - - def set(m0: Float, m1: Float, - m2: Float, m3: Float, - m4: Float, m5: Float): Mat2d = { - this.m0 = m0; this.m1 = m1 - this.m2 = m2; this.m3 = m3 - this.m4 = m4; this.m5 = m5 - this - } - - def set(m: Mat2d): Mat2d = - this.set(m.m0, m.m1, - m.m2, m.m3, - m.m4, m.m5) - - def set(m: Mat3): Mat2d = - this.set(m.m00, m.m01, - m.m10, m.m11, - m.m20, m.m21) - - /** Set this matrix to the identity */ - def setIdentity() = - this.set(1, 0, 0, 1, 0, 0) - - /** Invert this matrix into another Mat2d */ - def invertInto(m: Mat2d): Mat2d = { - var det = m0 * m3 - m1 * m2 - if(det == 0) - return null - det = 1/det - m.set( m3 * det, -m1 * det, - -m2 * det, m0 * det, - (m2 * m5 - m3 * m4) * det, - (m1 * m4 - m0 * m5) * det) - } - - /** Multiply a vector by this matrix */ - def *(v: Vec2): Vec2 = - Vec2(v.x * m0 + v.y * m2 + m4, - v.x * m1 + v.y * m3 + m5) - - /** Multiply this Mat2d by another Mat2d, creating a new Mat2d */ - def *(m: Mat2d): Mat2d = - multiplyInto(Mat2d(m), m) - - /** Destructively invert this matrix */ - def invert(): Mat2d = - invertInto(this) - - /** Compute the determinant of this matrix */ - def determinant: Float = - m0 * m3 - m1 * m2 - - /** Multiply this matrix by another matrix, into an output matrix */ - def multiplyInto(out: Mat2d, m: Mat2d): Mat2d = { - out.set(m0 * m.m0 + m2*m.m1, - m1 * m.m0 + m3*m.m1, - m0 * m.m2 + m2*m.m3, - m1 * m.m2 + m3*m.m3, - m0 * m.m4 + m2*m.m5 + m4, - m1 * m.m4 + m3*m.m5 + m5) - } - - /** Destructively multiply this matrix with another */ - def multiply(m: Mat2d): Mat2d = - multiplyInto(this, m) - - /** Rotate this matrix by the given angle, storing the result in an output matrix */ - def rotateInto(out: Mat2d, angle: Float): Mat2d = { - val s = Math.sin(angle).toFloat - val c = Math.cos(angle).toFloat - out.set(m0 * c + m2 * s, m1 * c + m3 * s, - m0 * -s + m2 * c, m1 * -s + m3 * c, - m4, m5) - } - - def rotate(angle: Float): Mat2d = - rotateInto(this, angle) - - def scaleInto(out: Mat2d, v: Vec2): Mat2d = - out.set(m0*v.x, m1*v.x, m2*v.y, m3*v.y, m4, m5) - - def scale(v: Vec2): Mat2d = - scaleInto(this, v) - - def translateInto(out: Mat2d, v: Vec2): Mat2d = - out.set(m0, m1, m2, m3, m0*v.x + m2*v.y + m4, m1*v.x + m3*v.y + m5) - - def translate(v: Vec2): Mat2d = - translateInto(this, v) - - override def toString: String = - s"Mat2d($m0, $m1, $m2,\n"+ - s" $m3, $m4, $m5)" - - def toArray(array: Array[Float]): Array[Float] = { - array(0) = m0; array(1) = m1; array(2) = m2 - array(3) = m3; array(4) = m4; array(5) = m5 - array - } -} - -object Mat2d { - def apply(): Mat2d = new Mat2d() - - def apply(m0: Float, m1: Float, - m2: Float, m3: Float, - m4: Float, m5: Float): Mat2d = - new Mat2d(m0, m1, m2, m3, m4, m5) - - def apply(m: Mat2d): Mat2d = - new Mat2d(m) - - def apply(m: Mat3): Mat2d = - new Mat2d(m) +package scryetek.vecmath + +/** + * Represents a 3x3 matrix with the bottom row set to 0 0 1. + */ +final class Mat2d( + var m00: Float, var m01: Float, var m02: Float, + var m10: Float, var m11: Float, var m12: Float) { + @inline + def this() = this(1, 0, 0, + 0, 1, 0) + + @inline + def set(m00: Float = m00, m01: Float = m01, m02: Float = m02, + m10: Float = m10, m11: Float = m11, m12: Float = m12): Mat2d = { + this.m00 = m00; this.m01 = m01; this.m02 = m02 + this.m10 = m10; this.m11 = m11; this.m12 = m12 + this + } + + def set(m: Mat2d): Mat2d = + Mat2d(m.m00, m.m01, m.m02, + m.m10, m.m11, m.m12) + + @inline + def inverted = { + var det = determinant + assert(det != 0, "Matrix is not invertable") + det = 1/det + Mat2d( m11*det, -m01*det, (m01*m12 - m02*m11)*det, + -m10*det, m00*det, (m02*m10 - m00*m12)*det) + } + + @inline + def invert(out: Mat2d = this) = { + var det = determinant + assert(det != 0, "Matrix is not invertable") + det = 1/det + out.set( m11*det, -m01*det, (m01*m12 - m02*m11)*det, + -m10*det, m00*det, (m02*m10 - m00*m12)*det) + } + + @inline + def determinant = + m00*m11 - m01*m10 + + /** Adds two matrices */ + @inline + def +(m: Mat2d): Mat2d = + Mat2d(m00 + m.m00, m01 + m.m01, m02 + m.m02, + m10 + m.m10, m11 + m.m11, m12 + m.m12) + + /** Destructively add another matrix to this matrix into the output matrix. */ + @inline + def add(m: Mat2d, out: Mat2d = this): Mat2d = + out.set(m00 + m.m00, m01 + m.m01, m02 + m.m02, + m10 + m.m10, m11 + m.m11, m12 + m.m12) + + /** Subtracts two matrices */ + @inline + def -(m: Mat2d): Mat2d = + Mat2d(m00 - m.m00, m01 - m.m01, m02 - m.m02, + m10 - m.m10, m11 - m.m11, m12 - m.m12) + + /** Destructively add another matrix to this matrix into the output matrix. */ + @inline + def sub(m: Mat2d, out: Mat2d = this): Mat2d = + out.set(m00 - m.m00, m01 - m.m01, m02 - m.m02, + m10 - m.m10, m11 - m.m11, m12 - m.m12) + + @inline + def *(v: Vec3): Vec3 = + Vec3(m00*v.x + m01*v.y + m02*v.z, + m10*v.x + m11*v.y + m12*v.z, + v.z) + + @inline + def *(s: Float): Mat2d = + Mat2d(m00*s, m01*s, m02*s, + m10*s, m11*s, m12*s) + + @inline + def scale(s: Float, out: Mat2d = this): Mat2d = + out.set(m00*s, m01*s, m02*s, + m10*s, m11*s, m12*s) + + @inline + def mul(v: Vec3, out: Vec3): Vec3 = + out.set(m00*v.x + m01*v.y + m02*v.z, + m10*v.x + m11*v.y + m12*v.z, + v.z) + + @inline + def mul(v: Vec3): Vec3 = + mul(v, v) + + @inline + def *(v: Vec2): Vec2 = + Vec2(v.x * m00 + v.y * m01 + m02, + v.x * m10 + v.y * m11 + m12) + + @inline + def mul(v: Vec2, out: Vec2): Vec2 = + out.set(v.x * m00 + v.y * m01 + m02, + v.x * m10 + v.y * m11 + m12) + + @inline + def mul(v: Vec2): Vec2 = + mul(v, v) + + @inline + def *(m: Mat2d): Mat2d = + Mat2d(m.m00*m00 + m.m10*m01, m.m01*m00 + m.m11*m01, m.m02*m00 + m.m12*m01 + m02, + m.m00*m10 + m.m10*m11, m.m01*m10 + m.m11*m11, m.m02*m10 + m.m12*m11 + m12) + + @inline + def postMultiply(m: Mat2d, out: Mat2d = this): Mat2d = + out.set(m.m00*m00 + m.m10*m01, m.m01*m00 + m.m11*m01, m.m02*m00 + m.m12*m01 + m02, + m.m00*m10 + m.m10*m11, m.m01*m10 + m.m11*m11, m.m02*m10 + m.m12*m11 + m12) + + @inline + def preMultiply(m: Mat2d, out: Mat2d = this): Mat2d = + out.set(m00*m.m00 + m10*m.m01, m01*m.m00 + m11*m.m01, m02*m.m00 + m12*m.m01 + m.m02, + m00*m.m10 + m10*m.m11, m01*m.m10 + m11*m.m11, m02*m.m10 + m12*m.m11 + m.m12) + + @inline + def toMat3 = + Mat3(m00, m01, m02, + m10, m11, m12, + 0, 0, 1) + + @inline + def toMat4 = + Mat4(m00, m01, m02, 0, + m10, m11, m12, 0, + 0, 0, 1, 0, + 0, 0, 0, 1) + + def copy(m00: Float = m00, m01: Float = m01, m02: Float = m02, + m10: Float = m10, m11: Float = m11, m12: Float = m12): Mat2d = + Mat2d(m00, m01, m02, + m10, m11, m12) + + override def toString = + s"Mat2d(${m00}f,${m01}f,${m02}f,${m10}f,${m11}f,${m12}f)" + + override def equals(o: Any): Boolean = o match { + case v: Mat2d => + m00 == v.m00 && m01 == v.m01 && m02 == v.m02 && + m10 == v.m10 && m11 == v.m11 && m12 == v.m12 + case _ => false + } + + override def hashCode: Int = + m00.hashCode() * 19 + m01.hashCode() * 23 + m02.hashCode() * 29 + + m10.hashCode() * 31 + m11.hashCode() * 37 + m12.hashCode() * 41 +} + +object Mat2d { + def apply(m00: Float, m01: Float, m02: Float, + m10: Float, m11: Float, m12: Float): Mat2d = + new Mat2d(m00, m01, m02, + m10, m11, m12) + + def apply() = new Mat2d() + + @inline + def scale2d(x: Float, y: Float): Mat2d = + Mat2d(x, 0, 0, + 0, y, 0) + + @inline + def scale2d(v: Vec2): Mat2d = + scale2d(v.x, v.y) + + @inline + def rotate2d(angle: Float): Mat2d = { + val c = math.cos(angle).toFloat + val s = math.sin(angle).toFloat + Mat2d(c, -s, 0, + s, c, 0) + } + + @inline + def translate2d(x: Float, y: Float): Mat2d = + Mat2d(1, 0, x, + 0, 1, y) + + @inline + def translate2d(t: Vec2): Mat2d = + translate2d(t.x, t.y) } \ No newline at end of file diff --git a/shared/src/main/scala/scryetek/vecmath/Mat3.scala b/shared/src/main/scala/scryetek/vecmath/Mat3.scala index bae9890..d614187 100644 --- a/shared/src/main/scala/scryetek/vecmath/Mat3.scala +++ b/shared/src/main/scala/scryetek/vecmath/Mat3.scala @@ -1,194 +1,582 @@ -package scryetek.vecmath - -/** - * Created by Matt on 06/07/2014. - */ -class Mat3(var m00: Float, var m01: Float, var m02: Float, - var m10: Float, var m11: Float, var m12: Float, - var m20: Float, var m21: Float, var m22: Float) { - def this() = - this(1, 0, 0, - 0, 1, 0, - 0, 0, 1) - - def this(m: Mat3) = - this(m.m00, m.m01, m.m02, - m.m10, m.m11, m.m12, - m.m20, m.m21, m.m22) - - def this(m: Mat4) = - this(m.m00, m.m01, m.m02, - m.m10, m.m11, m.m12, - m.m20, m.m21, m.m22) - - def set(m00: Float, m01: Float, m02: Float, - m10: Float, m11: Float, m12: Float, - m20: Float, m21: Float, m22: Float): Mat3 = { - this.m00 = m00; this.m01 = m01; this.m02 = m02 - this.m10 = m10; this.m11 = m11; this.m12 = m12 - this.m20 = m20; this.m21 = m21; this.m22 = m22 - this - } - - def set(m: Mat3): Mat3 = { - m00 = m.m00; m01 = m.m01; m02 = m.m02 - m10 = m.m10; m11 = m.m11; m12 = m.m12 - m20 = m.m20; m21 = m.m21; m22 = m.m22 - this - } - - def set(m: Mat4): Mat3 = { - m00 = m.m00; m01 = m.m01; m02 = m.m02 - m10 = m.m10; m11 = m.m11; m12 = m.m12 - m20 = m.m20; m21 = m.m21; m22 = m.m22 - this - } - - def *(v: Vec3): Vec3 = - Vec3(v.x * m00 + v.y * m10 + v.z * m20, - v.x * m01 + v.y * m11 + v.z * m21, - v.x * m02 + v.y * m12 + v.z * m22) - - def *(v: Vec2): Vec2 = { - val s = v.x * m02 + v.y * m12 + m22 - Vec2((v.x * m00 + v.y * m10 + m20) / s, - (v.x * m01 + v.y * m11 + m21) / s) - } - - def setIdentity: Mat3 = { - m00 = 1; m01 = 0; m02 = 0 - m10 = 0; m11 = 1; m12 = 0 - m20 = 0; m21 = 0; m22 = 1 - this - } - - def transposeInto(m: Mat3): Mat3 = - m.set(m00, m10, m20, - m01, m11, m21, - m02, m12, m22) - - def transpose: Mat3 = - transposeInto(this) - - def invertInto(m: Mat3): Mat3 = { - val b01 = m22*m11 - m12*m21 - val b11 = -m22*m10 + m12*m20 - val b21 = m21*m10 - m11*m20 - - var det = m00 * b01 + m01 * b11 + m02 * b21 - if(det == 0) - return null - det = 1.0f / det - - m.set(b01*det, (-m22*m01 + m02*m21) * det, ( m12*m01 - m02*m11)*det, - b11*det, ( m22*m00 - m02*m20) * det, (-m12*m00 + m02*m10)*det, - b21*det, (-m21*m00 + m01*m20) * det, ( m11*m00 - m01*m10)*det) - } - - def invert: Mat3 = invertInto(this) - - def adjointInto(m: Mat3): Mat3 = - m.set(m11*m22 - m12*m21, m02*m21 - m01*m22, m01*m12 - m02*m11, - m12*m20 - m10*m22, m00*m22 - m02*m20, m02*m10 - m00*m12, - m10*m21 - m11*m20, m01*m20 - m00*m21, m00*m11 - m01*m10) - - def adjoint: Mat3 = adjointInto(this) - - def determinant: Float = - m00*( m22*m11 - m12*m21) + - m01*(-m22*m10 + m12*m20) + - m02*( m21*m10 - m11*m20) - - def multiplyInto(out: Mat3, m: Mat3): Mat3 = - out.set(m.m00*m00 + m.m01*m10 + m.m02*m20, - m.m00*m01 + m.m01*m11 + m.m02*m21, - m.m00*m02 + m.m01*m12 + m.m02*m22, - - m.m10*m00 + m.m11*m10 + m.m12*m20, - m.m10*m01 + m.m11*m11 + m.m12*m21, - m.m10*m02 + m.m11*m12 + m.m12*m22, - - m.m20*m00 + m.m21*m10 + m.m22*m20, - m.m20*m01 + m.m21*m11 + m.m22*m21, - m.m20*m02 + m.m21*m12 + m.m22*m22) - - def multiply(m: Mat3): Mat3 = - multiplyInto(this, m) - - def translateInto(out: Mat3, v: Vec2): Mat3 = - out.set(m00, m01, m02, - m10, m11, m12, - v.x*m00 + v.y*m10 + m20, - v.x*m01 + v.y*m11 + m21, - v.x*m02 + v.y*m12 + m22) - - def translate(v: Vec2): Mat3 = - translateInto(this, v) - - def rotateInto(out: Mat3, angle: Float): Mat3 = { - val s = Math.sin(angle).toFloat - val c = Math.cos(angle).toFloat - out.set(c*m00 + s*m10, c*m01 + s*m11, c*m02 + s*m12, - c*m10 - s*m00, c*m11 - s*m01, c*m12 - s*m02, - m20, m21, m22) - } - - def rotate(angle: Float): Mat3 = - rotateInto(this, angle) - - def scaleInto(out: Mat3, v: Vec2): Mat3 = - out.set(v.x*m00, v.x*m01, v.x*m02, - v.y*m10, v.y*m11, v.y*m12, - m20, m21, m22) - - def scale(v: Vec2): Mat3 = - scaleInto(this, v) - - def set(q: Quat): Mat3 = { - val x2 = q.x + q.x - val y2 = q.y + q.y - val z2 = q.z + q.z - - val xx = q.x*x2 - val yx = q.y*x2 - val yy = q.y*y2 - val zx = q.z*x2 - val zy = q.z*y2 - val zz = q.z*z2 - val wx = q.w*x2 - val wy = q.w*y2 - val wz = q.w*z2 - - set(1 - yy - zz, yx + wz, zx - wy, - yx - wz, 1 - xx -zz, zy + wx, - zx + wy, 1 - xx - zz, zy + wx) - } - - // fromMat2d - - override def toString: String = - s"Mat3($m00, $m01, $m02,\n" + - s" $m10, $m11, $m12,\n" + - s" $m20, $m21, $m22)" - - def toArray(array: Array[Float]): Array[Float] = { - array(0) = m00; array(1) = m01; array(2) = m02 - array(3) = m10; array(4) = m11; array(5) = m12 - array(6) = m20; array(7) = m21; array(8) = m22 - array - } -} - -object Mat3 { - def apply(): Mat3 = new Mat3() - - def apply(m00: Float, m01: Float, m02: Float, - m10: Float, m11: Float, m12: Float, - m20: Float, m21: Float, m22: Float): Mat3 = - new Mat3(m00, m01, m02, - m10, m11, m12, - m20, m21, m22) - - def apply(m: Mat3): Mat3 = new Mat3(m) - def apply(m: Mat4): Mat3 = new Mat3(m) +package scryetek.vecmath + +final class Mat3( + var m00: Float, var m01: Float, var m02: Float, + var m10: Float, var m11: Float, var m12: Float, + var m20: Float, var m21: Float, var m22: Float +) { + @inline + def set(m00: Float = m00, m01: Float = m01, m02: Float = m02, + m10: Float = m10, m11: Float = m11, m12: Float = m12, + m20: Float = m20, m21: Float = m22, m22: Float = m22): Mat3 = { + this.m00 = m00; this.m01 = m01; this.m02 = m02 + this.m10 = m10; this.m11 = m11; this.m12 = m12 + this.m20 = m20; this.m21 = m21; this.m22 = m22 + this + } + + @inline + def set(m: Mat3): Mat3 = + set(m.m00, m.m01, m.m02, + m.m10, m.m11, m.m12, + m.m20, m.m21, m.m22) + + @inline + def this() = + this(1, 0, 0, + 0, 1, 0, + 0, 0, 1) + + @inline + def transposed = + Mat3(m00, m10, m20, + m01, m11, m21, + m02, m12, m22) + + @inline + def transpose(out: Mat3 = this): Mat3 = + out.set(m00, m10, m20, + m01, m11, m21, + m02, m12, m22) + + /** Adds two matrices */ + @inline + def +(m: Mat3): Mat3 = + Mat3(m00 + m.m00, m01 + m.m01, m02 + m.m02, + m10 + m.m10, m11 + m.m11, m12 + m.m12, + m20 + m.m20, m21 + m.m21, m22 + m.m22) + + /** Destructively add another matrix to this matrix into the output matrix. */ + @inline + def add(m: Mat3, out: Mat3 = this): Mat3 = + out.set(m00 + m.m00, m01 + m.m01, m02 + m.m02, + m10 + m.m10, m11 + m.m11, m12 + m.m12, + m20 + m.m20, m21 + m.m21, m22 + m.m22) + + /** Subtracts two matrices */ + @inline + def -(m: Mat3): Mat3 = + Mat3(m00 - m.m00, m01 - m.m01, m02 - m.m02, + m10 - m.m10, m11 - m.m11, m12 - m.m12, + m20 - m.m20, m21 - m.m21, m22 - m.m22) + + /** Destructively add another matrix to this matrix into the output matrix. */ + @inline + def sub(m: Mat3, out: Mat3 = this): Mat3 = + out.set(m00 - m.m00, m01 - m.m01, m02 - m.m02, + m10 - m.m10, m11 - m.m11, m12 - m.m12, + m20 - m.m20, m21 - m.m21, m22 - m.m22) + + @inline + def *(v: Vec3): Vec3 = + Vec3( + m00*v.x + m01*v.y + m02*v.z, + m10*v.x + m11*v.y + m12*v.z, + m20*v.x + m21*v.y + m22*v.z) + + @inline + def mul(v: Vec3, out: Vec3): Vec3 = + out.set(m00*v.x + m01*v.y + m02*v.z, + m10*v.x + m11*v.y + m12*v.z, + m20*v.x + m21*v.y + m22*v.z) + + @inline + def mul(v: Vec3): Vec3 = + mul(v, v) + + @inline + def *(s: Float): Mat3 = + Mat3(m00*s, m01*s, m02*s, + m10*s, m11*s, m12*s, + m20*s, m21*s, m22*s) + + @inline + def scale(s: Float, out: Mat3 = this): Mat3 = + out.set(m00*s, m01*s, m02*s, + m10*s, m11*s, m12*s, + m20*s, m21*s, m22*s) + + @inline + def *(b: Mat3): Mat3 = + Mat3(b.m00*m00 + b.m10*m01 + b.m20*m02, + b.m01*m00 + b.m11*m01 + b.m21*m02, + b.m02*m00 + b.m12*m01 + b.m22*m02, + + b.m00*m10 + b.m10*m11 + b.m20*m12, + b.m01*m10 + b.m11*m11 + b.m21*m12, + b.m02*m10 + b.m12*m11 + b.m22*m12, + + b.m00*m20 + b.m10*m21 + b.m20*m22, + b.m01*m20 + b.m11*m21 + b.m21*m22, + b.m02*m20 + b.m12*m21 + b.m22*m22) + + @inline + def *(q: Quat): Mat3 = { + val xx = q.x*q.x; val xy = q.x*q.y; val xz = q.x*q.z; val xw = q.x*q.w; val ww = q.w*q.w + val yy = q.y*q.y; val yz = q.y*q.z; val yw = q.y*q.w; val zz = q.z*q.z; val zw = q.z*q.w + + val b00 = 1-2*yy-2*zz; val b01 = 2*(xy-zw); val b02 = 2*(xz+yw) + val b10 = 2*(xy+zw); val b11 = 1-2*xx-2*zz; val b12 = 2*(yz-xw) + val b20 = 2*(xz-yw); val b21 = 2*(yz+xw); val b22 = 1-2*xx-2*yy + + Mat3(b00*m00 + b10*m01 + b20*m02, + b01*m00 + b11*m01 + b21*m02, + b02*m00 + b12*m01 + b22*m02, + + b00*m10 + b10*m11 + b20*m12, + b01*m10 + b11*m11 + b21*m12, + b02*m10 + b12*m11 + b22*m12, + + b00*m20 + b10*m21 + b20*m22, + b01*m20 + b11*m21 + b21*m22, + b02*m20 + b12*m21 + b22*m22) + } + @inline + def postMultiply(m: Mat3): Mat3 = + postMultiply(m.m00, m.m01, m.m02, + m.m10, m.m11, m.m12, + m.m20, m.m21, m.m22) + + @inline + def postMultiply(m: Mat3, out: Mat3): Mat3 = + postMultiply(m.m00, m.m01, m.m02, + m.m10, m.m11, m.m12, + m.m20, m.m21, m.m22, out) + + @inline + def postMultiply(b00: Float, b01: Float, b02: Float, + b10: Float, b11: Float, b12: Float, + b20: Float, b21: Float, b22: Float, + out: Mat3 = this): Mat3 = + out.set(b00*m00 + b10*m01 + b20*m02, + b01*m00 + b11*m01 + b21*m02, + b02*m00 + b12*m01 + b22*m02, + + b00*m10 + b10*m11 + b20*m12, + b01*m10 + b11*m11 + b21*m12, + b02*m10 + b12*m11 + b22*m12, + + b00*m20 + b10*m21 + b20*m22, + b01*m20 + b11*m21 + b21*m22, + b02*m20 + b12*m21 + b22*m22) + + @inline + def preMultiply(m: Mat3, out: Mat3): Mat3 = + preMultiply(m.m00, m.m01, m.m02, + m.m10, m.m11, m.m12, + m.m20, m.m21, m.m22, out) + + @inline + def preMultiply(m: Mat3): Mat3 = + preMultiply(m.m00, m.m01, m.m02, + m.m10, m.m11, m.m12, + m.m20, m.m21, m.m22, this) + + @inline + def preMultiply(b00: Float, b01: Float, b02: Float, + b10: Float, b11: Float, b12: Float, + b20: Float, b21: Float, b22: Float, + out: Mat3 = this): Mat3 = + out.set(m00*b00 + m10*b01 + m20*b02, + m01*b00 + m11*b01 + m21*b02, + m02*b00 + m12*b01 + m22*b02, + + m00*b10 + m10*b11 + m20*b12, + m01*b10 + m11*b11 + m21*b12, + m02*b10 + m12*b11 + m22*b12, + + m00*b20 + m10*b21 + m20*b22, + m01*b20 + m11*b21 + m21*b22, + m02*b20 + m12*b21 + m22*b22) + + @inline + def preRotate(angle: Float, x: Float, y: Float, z: Float): Mat3 = { + val s = Math.sin(angle).toFloat + val c = Math.cos(angle).toFloat + val t = 1-c + + preMultiply(x*x*t + c, x*y*t - z*s, x*z*t+y*s, + y*x*t + z*s, y*y*t + c, y*z*t-x*s, + x*z*t - y*s, y*z*t + x*s, z*z*t + c) + } + + @inline + def preRotate(angle: Float, axis: Vec3): Mat3 = + preRotate(angle, axis.x, axis.y, axis.z) + + @inline + def postRotate(angle: Float, x: Float, y: Float, z: Float): Mat3 = { + val s = Math.sin(angle).toFloat + val c = Math.cos(angle).toFloat + val t = 1-c + + postMultiply(x*x*t + c, x*y*t - z*s, x*z*t+y*s, + y*x*t + z*s, y*y*t + c, y*z*t-x*s, + x*z*t - y*s, y*z*t + x*s, z*z*t + c) + } + + @inline + def postRotate(angle: Float, axis: Vec3): Mat3 = + postRotate(angle, axis.x, axis.y, axis.z) + + @inline + def preRotateQuat(x: Float, y: Float, z: Float, w: Float): Mat3 = { + val xx = x*x; val xy = x*y; val xz = x*z; val xw = x*w; val ww = w*w + val yy = y*y; val yz = y*z; val yw = y*w; val zz = z*z; val zw = z*w + preMultiply(1-2*yy-2*zz, 2*(xy-zw), 2*(xz+yw), + 2*(xy+zw), 1-2*xx-2*zz, 2*(yz-xw), + 2*(xz-yw), 2*(yz+xw), 1-2*xx-2*yy) + } + + @inline + def preRotateQuat(q: Quat): Mat3 = + preRotateQuat(q.x, q.y, q.z, q.w) + + @inline + def postRotateQuat(x: Float, y: Float, z: Float, w: Float): Mat3 = { + val xx = x*x; val xy = x*y; val xz = x*z; val xw = x*w; val ww = w*w + val yy = y*y; val yz = y*z; val yw = y*w; val zz = z*z; val zw = z*w + postMultiply(1-2*yy-2*zz, 2*(xy-zw), 2*(xz+yw), + 2*(xy+zw), 1-2*xx-2*zz, 2*(yz-xw), + 2*(xz-yw), 2*(yz+xw), 1-2*xx-2*yy) + } + + @inline + def postRotateQuat(q: Quat): Mat3 = + postRotateQuat(q.x, q.y, q.z, q.w) + + @inline + def preScale(x: Float, y: Float, z: Float): Mat3 = + preMultiply(x, 0, 0, + 0, y, 0, + 0, 0, z) + + /** + * Destructively pre-multiplies a rotation around the x-axis into this matrix. + */ + @inline + def preRotateX(angle: Float): Mat3 = { + val s = Math.sin(angle).toFloat + val c = Math.cos(angle).toFloat + preMultiply(1, 0, 0, + 0, c, -s, + 0, s, c) + } + + /** + * Destructively post-multiplies a rotation around the x-axis into this matrix. + */ + @inline + def postRotateX(angle: Float): Mat3 = { + val s = Math.sin(angle).toFloat + val c = Math.cos(angle).toFloat + postMultiply(1, 0, 0, + 0, c, -s, + 0, s, c) + } + + /** + * Destructively pre-multiplies a rotation around the y-axis into this matrix. + */ + @inline + def preRotateY(angle: Float): Mat3 = { + val s = Math.sin(angle).toFloat + val c = Math.cos(angle).toFloat + preMultiply( c, 0, s, + 0, 1, 0, + -s, 0, c) + } + + /** + * Destructively post-multiplies a rotation around the y-axis into this matrix. + */ + @inline + def postRotateY(angle: Float): Mat3 = { + val s = Math.sin(angle).toFloat + val c = Math.cos(angle).toFloat + postMultiply( c, 0, s, + 0, 1, 0, + -s, 0, c) + } + + /** + * Destructively pre-multiplies a rotation around the z-axis into this matrix. + */ + @inline + def preRotateZ(angle: Float): Mat3 = { + val s = Math.sin(angle).toFloat + val c = Math.cos(angle).toFloat + preMultiply( c, -s, 0, + s, c, 0, + 0, 0, 1) + } + + /** + * Destructively post-multiplies a rotation around the z-axis into this matrix. + */ + @inline + def postRotateZ(angle: Float): Mat3 = { + val s = Math.sin(angle).toFloat + val c = Math.cos(angle).toFloat + postMultiply( c, -s, 0, + s, c, 0, + 0, 0, 1) + } + + @inline + def preScale(s: Vec3): Mat3 = + preScale(s.x, s.y, s.z) + + @inline + def postScale(x: Float, y: Float, z: Float): Mat3 = + postMultiply(x, 0, 0, + 0, y, 0, + 0, 0, z) + @inline + def postScale(s: Vec3): Mat3 = + postScale(s.x, s.y, s.z) + + @inline + def determinant = { + val b01 = m22*m11 - m12*m21 + val b11 = -m22*m10 + m12*m20 + val b21 = m21*m10 - m11*m20 + + m00 * b01 + m01 * b11 + m02 * b21 + } + + @inline + def inverted = { + val b01 = m22*m11 - m12*m21 + val b11 = -m22*m10 + m12*m20 + val b21 = m21*m10 - m11*m20 + + var det = m00 * b01 + m01 * b11 + m02 * b21 + assert(det != 0, "Matrix is not invertable") + det = 1.0f / det + + Mat3(b01*det, (-m22*m01 + m02*m21) * det, ( m12*m01 - m02*m11)*det, + b11*det, ( m22*m00 - m02*m20) * det, (-m12*m00 + m02*m10)*det, + b21*det, (-m21*m00 + m01*m20) * det, ( m11*m00 - m01*m10)*det) + } + + @inline + def invert(out: Mat3 = this): Mat3 = { + val b01 = m22*m11 - m12*m21 + val b11 = -m22*m10 + m12*m20 + val b21 = m21*m10 - m11*m20 + + var det = m00 * b01 + m01 * b11 + m02 * b21 + assert(det != 0, "Matrix is not invertable") + det = 1.0f / det + + out.set(b01*det, (-m22*m01 + m02*m21) * det, ( m12*m01 - m02*m11)*det, + b11*det, ( m22*m00 - m02*m20) * det, (-m12*m00 + m02*m10)*det, + b21*det, (-m21*m00 + m01*m20) * det, ( m11*m00 - m01*m10)*det) + } + + @inline + def adjointed = + Mat3(m11*m22 - m12*m21, m02*m21 - m01*m22, m01*m12 - m02*m11, + m12*m20 - m10*m22, m00*m22 - m02*m20, m02*m10 - m00*m12, + m10*m21 - m11*m20, m01*m20 - m00*m21, m00*m11 - m01*m10) + + @inline + def adjoint(out: Mat3 = this) = + out.set(m11*m22 - m12*m21, m02*m21 - m01*m22, m01*m12 - m02*m11, + m12*m20 - m10*m22, m00*m22 - m02*m20, m02*m10 - m00*m12, + m10*m21 - m11*m20, m01*m20 - m00*m21, m00*m11 - m01*m10) + + @inline + def toMat4 = + Mat4(m00, m01, m02, 0, + m10, m11, m12, 0, + m20, m21, m22, 0, + 0, 0, 0, 1) + + @inline + def toQuat = { + val tr = m00 + m11 + m22 + + if (tr > 0) { + val S = math.sqrt(tr + 1.0f).toFloat * 2 // S=4*qw + Quat((m21 - m12) / S, (m02 - m20) / S, (m10 - m01) / S, 0.25f * S).normalized + } else if ((m00 > m11) & (m00 > m22)) { + val S = math.sqrt(1.0 + m00 - m11 - m22).toFloat * 2 // S=4*qx + Quat(0.25f * S, (m01 + m10) / S, (m02 + m20) / S,(m21 - m12) / S).normalized + } else if (m11 > m22) { + val S = math.sqrt(1.0 + m11 - m00 - m22).toFloat * 2 // S=4*qy + Quat((m01 + m10) / S, 0.25f * S, (m12 + m21) / S, (m02 - m20) / S).normalized + } else { + val S = math.sqrt(1.0 + m22 - m00 - m11).toFloat * 2; // S=4*qz + Quat((m02 + m20) / S, (m12 + m21) / S, 0.25f * S, (m10 - m01) / S).normalized + } + } + + @inline + def toMat2d: Mat2d = + Mat2d(m00, m01, m02, + m10, m11, m12) + + + @inline + def copy(m00: Float = m00, m01: Float = m01, m02: Float = m02, + m10: Float = m10, m11: Float = m11, m12: Float = m12, + m20: Float = m20, m21: Float = m22, m22: Float = m22): Mat3 = + Mat3(m00, m01, m02, + m10, m11, m12, + m20, m21, m22) + + override def toString = + s"Mat3(${m00}f,${m01}f,${m02}f,${m10}f,${m11}f,${m12}f,${m20}f,${m21}f,${m22}f)" + + override def equals(o: Any): Boolean = o match { + case m: Mat3 => + m00 == m.m00 && m01 == m.m01 && m02 == m.m02 && + m10 == m.m10 && m11 == m.m11 && m12 == m.m12 && + m20 == m.m20 && m21 == m.m21 && m22 == m.m22 + case _ => false + } + + override def hashCode: Int = + m00.hashCode()*19 + m01.hashCode()*23 + m02.hashCode()*29 + + m10.hashCode()*31 + m11.hashCode()*37 + m12.hashCode()*41 + + m20.hashCode()*43 + m21.hashCode()*47 + m22.hashCode()*53 +} + +object Mat3 { + def apply(): Mat3 = + new Mat3() + + def apply(m00: Float, m01: Float, m02: Float, + m10: Float, m11: Float, m12: Float, + m20: Float, m21: Float, m22: Float): Mat3 = + new Mat3(m00, m01, m02, + m10, m11, m12, + m20, m21, m22) + + @inline + def rotateQuat(q: Quat): Mat3 = + rotateQuat(q.x, q.y, q.z, q.w) + + @inline + def rotateQuat(x: Float, y: Float, z: Float, w: Float): Mat3 = { + val xx = x*x; val xy = x*y; val xz = x*z; val xw = x*w; val ww = w*w; + val yy = y*y; val yz = y*z; val yw = y*w; val zz = z*z; val zw = z*w; + Mat3(1-2*yy-2*zz, 2*(xy-zw), 2*(xz+yw), + 2*(xy+zw), 1-2*xx-2*zz, 2*(yz-xw), + 2*(xz-yw), 2*(yz+xw), 1-2*xx-2*yy) + } + + /** + * Returns the rotation matrix about the given angle and axis. + * @param angle the angle to rotate, in radians. + * @param x the x-component of the axis vector to rotate around, must be normalized. + * @param y the y-component of the axis vector to rotate around, must be normalized. + * @param z the z-component of the axis vector to rotate around, must be normalized. + */ + @inline + def rotate(angle: Float, x: Float, y: Float, z: Float): Mat3 = { + val s = Math.sin(angle).toFloat + val c = Math.cos(angle).toFloat + val t = 1-c + + Mat3(x*x*t + c, x*y*t - z*s, x*z*t+y*s, + y*x*t + z*s, y*y*t + c, y*z*t-x*s, + x*z*t - y*s, y*z*t + x*s, z*z*t + c) + } + + /** + * Returns the rotation matrix about the given angle and axis. + * @param angle the angle to rotate, in radians. + * @param axis the axis vector to rotate around, must be normalized. + */ + @inline + def rotate(angle: Float, axis: Vec3): Mat3 = + rotate(angle, axis.x, axis.y, axis.z) + + /** + * Returns the rotation matrix rotating around the X-axis. + */ + @inline + def rotateX(angle: Float): Mat3 = { + val s = Math.sin(angle).toFloat + val c = Math.cos(angle).toFloat + Mat3(1, 0, 0, + 0, c, -s, + 0, s, c) + } + + /** + * Returns the rotation matrix rotating around the Y-axis. + */ + @inline + def rotateY(angle: Float): Mat3 = { + val s = Math.sin(angle).toFloat + val c = Math.cos(angle).toFloat + Mat3( c, 0, s, + 0, 1, 0, + -s, 0, c) + } + + /** + * Returns the rotation matrix rotating around the Z-axis. + */ + @inline + def rotateZ(angle: Float): Mat3 = { + val s = Math.sin(angle).toFloat + val c = Math.cos(angle).toFloat + Mat3( c, -s, 0, + s, c, 0, + 0, 0, 1) + } + + /** + * Returns a scale matrix + * @param x the x-scale + * @param y the y-scale + * @param z the z-scale + */ + @inline + def scale(x: Float, y: Float, z: Float): Mat3 = + Mat3(x, 0, 0, + 0, y, 0, + 0, 0, z) + + /** + * Returns a scale matrix + * @param s the scale vector + */ + @inline + def scale(s: Vec3): Mat3 = + scale(s.x, s.y, s.z) + + @inline + def scale2d(x: Float, y: Float): Mat3 = + Mat3(x, 0, 0, + 0, y, 0, + 0, 0, 1) + + @inline + def scale2d(v: Vec2): Mat3 = + scale2d(v.x, v.y) + + @inline + def rotate2d(angle: Float): Mat3 = { + val c = math.cos(angle).toFloat + val s = math.sin(angle).toFloat + Mat3(c, -s, 0, + s, c, 0, + 0, 0, 1) + } + + @inline + def translate2d(x: Float, y: Float): Mat3 = + Mat3(1, 0, x, + 0, 1, y, + 0, 0, 1) + + @inline + def translate2d(t: Vec2): Mat3 = + translate2d(t.x, t.y) } \ No newline at end of file diff --git a/shared/src/main/scala/scryetek/vecmath/Mat4.scala b/shared/src/main/scala/scryetek/vecmath/Mat4.scala index 575f221..9ccdfde 100644 --- a/shared/src/main/scala/scryetek/vecmath/Mat4.scala +++ b/shared/src/main/scala/scryetek/vecmath/Mat4.scala @@ -1,418 +1,898 @@ -package scryetek.vecmath - -class Mat4(var m00: Float, var m01: Float, var m02: Float, var m03: Float, - var m10: Float, var m11: Float, var m12: Float, var m13: Float, - var m20: Float, var m21: Float, var m22: Float, var m23: Float, - var m30: Float, var m31: Float, var m32: Float, var m33: Float) { - - def this() = this(1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1) - - def this(m: Mat4) = - this(m.m00, m.m01, m.m02, m.m03, - m.m10, m.m11, m.m12, m.m13, - m.m20, m.m21, m.m22, m.m23, - m.m30, m.m31, m.m32, m.m33) - - def set(m00: Float, m01: Float, m02: Float, m03: Float, - m10: Float, m11: Float, m12: Float, m13: Float, - m20: Float, m21: Float, m22: Float, m23: Float, - m30: Float, m31: Float, m32: Float, m33: Float): Mat4 = { - this.m00 = m00; this.m01 = m01; this.m02 = m02; this.m03 = m03 - this.m10 = m10; this.m11 = m11; this.m12 = m12; this.m13 = m13 - this.m20 = m20; this.m21 = m21; this.m22 = m22; this.m23 = m23 - this.m30 = m30; this.m31 = m31; this.m32 = m32; this.m33 = m33 - this - } - - def set(m: Mat4): Mat4 = { - m00 = m.m00; m01 = m.m01; m02 = m.m02; m03 = m.m03 - m10 = m.m10; m11 = m.m11; m12 = m.m12; m13 = m.m13 - m20 = m.m20; m21 = m.m21; m22 = m.m22; m23 = m.m23 - m30 = m.m30; m31 = m.m31; m32 = m.m32; m33 = m.m33 - this - } - - - /** Multiply a vector by this matrix */ - def *(v: Vec4): Vec4 = - Vec4( - v.x * m00 + v.y * m10 + v.z * m20 + v.w * m30, - v.x * m01 + v.y * m11 + v.z * m21 + v.w * m31, - v.x * m02 + v.y * m12 + v.z * m22 + v.w * m32, - v.x * m03 + v.y * m13 + v.z * m23 + v.w * m33) - - /** Multiply a vector by this matrix */ - def *(v: Vec3): Vec3 = { - val x = v.x * m00 + v.y * m10 + v.z * m20 + m30 - val y = v.x * m01 + v.y * m11 + v.z * m21 + m31 - val z = v.x * m02 + v.y * m12 + v.z * m22 + m32 - val w = 1/(v.x * m03 + v.y * m13 + v.z * m23 + m33) - Vec3(x*w, y*w, z*w) - } - - def setIdentity: Mat4 = { - m00 = 1; m01 = 0; m02 = 0; m03 = 0 - m10 = 0; m11 = 1; m12 = 0; m13 = 0 - m20 = 0; m21 = 0; m22 = 1; m23 = 0 - m30 = 0; m31 = 0; m32 = 0; m33 = 1 - this - } - - def transposeInto(m: Mat4): Mat4 = - m.set(m00, m10, m20, m30, - m01, m11, m21, m31, - m02, m12, m22, m32, - m03, m13, m33, m33) - - def transpose: Mat4 = - transposeInto(this) - - def invertInto(m: Mat4): Mat4 = { - val b00 = m00*m11 - m01*m10 - val b01 = m00*m12 - m02*m10 - val b02 = m00*m13 - m03*m10 - val b03 = m01*m12 - m02*m11 - val b04 = m01*m13 - m03*m11 - val b05 = m02*m13 - m03*m12 - val b06 = m20*m31 - m21*m30 - val b07 = m20*m32 - m22*m30 - val b08 = m20*m33 - m23*m30 - val b09 = m21*m32 - m22*m31 - val b10 = m21*m33 - m23*m31 - val b11 = m22*m33 - m23*m32 - - var det = b00*b11 - b01*b10 + b02*b09 + b03*b08 - b04*b07 + b05*b06 - if(det == 0) - return null - det = 1.0f / det - - m.set((m11*b11 - m12*b10 + m13*b09)*det, - (m02*b10 - m01*b11 - m03*b09)*det, - (m31*b05 - m32*b04 + m33*b03)*det, - (m22*b04 - m21*b05 - m23*b03)*det, - (m12*b08 - m10*b11 - m13*b07)*det, - (m00*b11 - m02*b08 + m03*b07)*det, - (m32*b02 - m30*b05 - m33*b01)*det, - (m20*b05 - m22*b02 + m23*b01)*det, - (m10*b10 - m11*b08 + m13*b06)*det, - (m01*b08 - m00*b10 - m03*b06)*det, - (m30*b04 - m31*b02 + m33*b00)*det, - (m21*b02 - m20*b04 - m23*b00)*det, - (m11*b07 - m10*b09 - m12*b06)*det, - (m00*b09 - m01*b07 + m02*b06)*det, - (m31*b01 - m30*b03 - m32*b00)*det, - (m20*b03 - m21*b01 + m22*b00)*det) - } - - def invert: Mat4 = - invertInto(this) - - def adjointInto(m: Mat4): Mat4 = - m.set( m11*(m22*m33 - m23*m32) - m21*(m12*m33 - m13*m32) + m31*(m12*m23 - m13*m22), - -(m01*(m22*m33 - m23*m32) - m21*(m02*m33 - m03*m32) + m31*(m02*m23 - m03*m22)), - m01*(m12*m33 - m13*m32) - m11*(m02*m33 - m03*m32) + m31*(m02*m13 - m03*m12), - -(m01*(m12*m23 - m13*m22) - m11*(m02*m23 - m03*m22) + m21*(m02*m13 - m03*m12)), - -(m10*(m22*m33 - m23*m32) - m20*(m12*m33 - m13*m32) + m30*(m12*m23 - m13*m22)), - m00*(m22*m33 - m23*m32) - m20*(m02*m33 - m03*m32) + m30*(m02*m23 - m03*m22), - -(m00*(m12*m33 - m13*m32) - m10*(m02*m33 - m03*m32) + m30*(m02*m13 - m03*m12)), - m00*(m12*m23 - m13*m22) - m10*(m02*m23 - m03*m22) + m20*(m02*m13 - m03*m12), - m10*(m21*m33 - m23*m31) - m20*(m11*m33 - m13*m31) + m30*(m11*m23 - m13*m21), - -(m00*(m21*m33 - m23*m31) - m20*(m01*m33 - m03*m31) + m30*(m01*m23 - m03*m21)), - m00*(m11*m33 - m13*m31) - m10*(m01*m33 - m03*m31) + m30*(m01*m13 - m03*m11), - -(m00*(m11*m23 - m13*m21) - m10*(m01*m23 - m03*m21) + m20*(m01*m13 - m03*m11)), - -(m10*(m21*m32 - m22*m31) - m20*(m11*m32 - m12*m31) + m30*(m11*m22 - m12*m21)), - m00*(m21*m32 - m22*m31) - m20*(m01*m32 - m02*m31) + m30*(m01*m22 - m02*m21), - -(m00*(m11*m32 - m12*m31) - m10*(m01*m32 - m02*m31) + m30*(m01*m12 - m02*m11)), - m00*(m11*m22 - m12*m21) - m10*(m01*m22 - m02*m21) + m20*(m01*m12 - m02*m11)) - - def adjoint: Mat4 = - adjointInto(this) - - def determinant: Float = { - val b00 = m00*m11 - m01*m10 - val b01 = m00*m12 - m02*m10 - val b02 = m00*m13 - m03*m10 - val b03 = m01*m12 - m02*m11 - val b04 = m01*m13 - m03*m11 - val b05 = m02*m13 - m03*m12 - val b06 = m20*m31 - m21*m30 - val b07 = m20*m32 - m22*m30 - val b08 = m20*m33 - m23*m30 - val b09 = m21*m32 - m22*m31 - val b10 = m21*m33 - m23*m31 - val b11 = m22*m33 - m23*m32 - - b00*b11 - b01*b10 + b02*b09 + b03*b08 - b04*b07 + b05*b06 - } - - def multiplyInto(out: Mat4, m: Mat4): Mat4 = - out.set(m.m00*m00 + m.m01*m10 + m.m02*m20 + m.m03*m30, - m.m00*m01 + m.m01*m11 + m.m02*m21 + m.m03*m31, - m.m00*m02 + m.m01*m12 + m.m02*m22 + m.m03*m32, - m.m00*m03 + m.m01*m13 + m.m02*m23 + m.m03*m33, - m.m10*m00 + m.m11*m10 + m.m12*m20 + m.m13*m30, - m.m10*m01 + m.m11*m11 + m.m12*m21 + m.m13*m31, - m.m10*m02 + m.m11*m12 + m.m12*m22 + m.m13*m32, - m.m10*m03 + m.m11*m13 + m.m12*m23 + m.m13*m33, - m.m20*m00 + m.m21*m10 + m.m22*m20 + m.m23*m30, - m.m20*m01 + m.m21*m11 + m.m22*m21 + m.m23*m31, - m.m20*m02 + m.m21*m12 + m.m22*m22 + m.m23*m32, - m.m20*m03 + m.m21*m13 + m.m22*m23 + m.m23*m33, - m.m30*m00 + m.m31*m10 + m.m32*m20 + m.m33*m30, - m.m30*m01 + m.m31*m11 + m.m32*m21 + m.m33*m31, - m.m30*m02 + m.m31*m12 + m.m32*m22 + m.m33*m32, - m.m30*m03 + m.m31*m13 + m.m32*m23 + m.m33*m33) - - def multiplyInto(out: Mat4, - b00: Float, b01: Float, b02: Float, b03: Float, - b10: Float, b11: Float, b12: Float, b13: Float, - b20: Float, b21: Float, b22: Float, b23: Float, - b30: Float, b31: Float, b32: Float, b33: Float) = - out.set(b00*m00 + b01*m10 + b02*m20 + b03*m30, - b00*m01 + b01*m11 + b02*m21 + b03*m31, - b00*m02 + b01*m12 + b02*m22 + b03*m32, - b00*m03 + b01*m13 + b02*m23 + b03*m33, - b10*m00 + b11*m10 + b12*m20 + b13*m30, - b10*m01 + b11*m11 + b12*m21 + b13*m31, - b10*m02 + b11*m12 + b12*m22 + b13*m32, - b10*m03 + b11*m13 + b12*m23 + b13*m33, - b20*m00 + b21*m10 + b22*m20 + b23*m30, - b20*m01 + b21*m11 + b22*m21 + b23*m31, - b20*m02 + b21*m12 + b22*m22 + b23*m32, - b20*m03 + b21*m13 + b22*m23 + b23*m33, - b30*m00 + b31*m10 + b32*m20 + b33*m30, - b30*m01 + b31*m11 + b32*m21 + b33*m31, - b30*m02 + b31*m12 + b32*m22 + b33*m32, - b30*m03 + b31*m13 + b32*m23 + b33*m33) - - def multiply(m: Mat4): Mat4 = - multiplyInto(this, m) - - def translateInto(out: Mat4, v: Vec3): Mat4 = { - val x = v.x; val y = v.y; val z = v.z - out.set(m00, m01, m02, m03, - m10, m11, m12, m13, - m20, m21, m22, m23, - m00*x + m10*y + m20*z + m30, - m01*x + m11*y + m21*z + m31, - m02*x + m12*y + m22*z + m32, - m03*x + m13*y + m23*z + m33) - } - - def translate(v: Vec3): Mat4 = - translateInto(this, v) - - def scaleInto(m: Mat4, v: Vec3): Mat4 = { - val x = v.x; val y = v.y; val z = v.z - m.set(m00*x, m01*x, m02*x, m03*x, - m10*y, m11*y, m12*y, m13*y, - m20*z, m21*z, m22*z, m23*z, - m30, m01, m02, m03) - } - - def scale(v: Vec3): Mat4 = - scaleInto(this, v) - - def rotateInto(m: Mat4, angle: Float, axis: Vec3): Mat4 = { - val axis2 = new Vec3(axis).normalize - val x = axis2.x; val y = axis2.y; val z = axis2.z - val s = Math.sin(angle).toFloat - val c = Math.cos(angle).toFloat - val t = 1-c - - val b00 = x*x*t + c; val b01 = y*x*t + z*s; val b02 = z*x*t - y*s - val b10 = x*y*t - z*s; val b11 = y*y*t + c; val b12 = z*y*t + x*s - val b20 = x*z*t + y*s; val b21 = y*z*t - x*s; val b22 = z*z*t + c - - m.set(m00*b00 + m10*b01 + m20*b02, - m01*b00 + m11*b01 + m21*b02, - m02*b00 + m12*b01 + m22*b02, - m03*b00 + m13*b01 + m23*b02, - - m00*b10 + m10*b11 + m20*b12, - m01*b10 + m11*b11 + m21*b12, - m02*b10 + m12*b11 + m22*b12, - m03*b10 + m13*b11 + m23*b12, - - m00*b20 + m10*b21 + m20*b22, - m01*b20 + m11*b21 + m21*b22, - m02*b20 + m12*b21 + m22*b22, - m03*b20 + m13*b21 + m23*b22, - - m30, m31, m32, m33) - } - - def rotate(angle: Float, axis: Vec3): Mat4 = - rotateInto(this, angle, axis) - - def rotateXInto(m: Mat4, angle: Float): Mat4 = { - val s = Math.sin(angle).toFloat - val c = Math.cos(angle).toFloat - - m.set(m00, m01, m02, m03, - m10*c + m20*s, m11*c + m21*s, m12*c + m22*s, m13*c + m23*s, - m20*c - m10*s, m21*c - m11*s, m22*c - m12*s, m23*c - m13*s, - m30, m31, m33, m33) - } - - def rotateX(angle: Float): Mat4 = - rotateXInto(this, angle) - - def rotateYInto(m: Mat4, angle: Float): Mat4 = { - val s = Math.sin(angle).toFloat - val c = Math.cos(angle).toFloat - - m.set(m00*c - m20*s, m01*c - m21*s, m02*c - m22*s, m03*c - m23*s, - m10, m11, m12, m13, - m00*s + m20*c, m01*s + m21*c, m02*s + m22*c, m03*s + m23*c, - m30, m31, m32, m33) - } - - def rotateY(angle: Float): Mat4 = - rotateYInto(this, angle) - - def rotateZInto(m: Mat4, angle: Float): Mat4 = { - val s = Math.sin(angle).toFloat - val c = Math.cos(angle).toFloat - - m.set(m00*c + m10*s, m01*c + m11*s, m20*s + m12*s, m03*c + m13*s, - m10*c - m00*s, m11*c - m01*s, m12*c - m02*s, m13*c - m03*s, - m20, m21, m22, m23, - m30, m31, m32, m33) - } - - def rotateZ(angle: Float): Mat4 = - rotateZInto(this, angle) - - /* - def fromRotationTranslation - */ - - def set(q: Quat): Mat4 = { - val x2 = q.x + q.x - val y2 = q.y + q.y - val z2 = q.z + q.z - - val xx = q.x*x2 - val yx = q.y*x2 - val yy = q.y*y2 - val zx = q.z*x2 - val zy = q.z*y2 - val zz = q.z*z2 - val wx = q.w*x2 - val wy = q.w*y2 - val wz = q.w*z2 - - set(1 - yy - zz, yx + wz, zx - wy, 0, - yx - wz, 1 - xx -zz, zy + wx, 0, - zx + wy, 1 - xx - zz, zy + wx, 0, - 0, 0, 0, 1) - } - - def lookAt(eye: Vec3, center: Vec3, up: Vec3): Mat4 = { - val z = new Vec3(eye).sub(center).normalize - val x = new Vec3(up).cross(z).normalize - val y = new Vec3(z).cross(x).normalize - - set( x.x, y.x, z.x, 0, - x.y, y.y, z.y, 0, - x.z, y.z, z.z, 0, - -(x*eye), -(y*eye), -(z*eye), 1) - } - - def frustum(left: Float, right: Float, bottom: Float, top: Float, near: Float, far: Float): Mat4 = { - val rl = 1 / (right-left) - val tb = 1 / (top-bottom) - val nf = 1 / (near-far) - - set((near*2)*rl, 0, 0, 0, - 0, (near*2)*tb, 0 , 0, - (right+left)*rl, (top+bottom)*tb, (far+near)*nf, -1, - -1, 0, (far*near)*nf, 0) - } - - def perspective(fovy: Float, aspect: Float, near: Float, far: Float): Mat4 = { - val f = 1.0f / Math.tan(fovy / 2).toFloat - val nf = 1 / (near-far) - - set(f/aspect, 0, 0, 0, - 0, f, 0, 0, - 0, 0, (far + near)*nf, -1, - 0, 0, 2*(far*near)*nf, 0) - } - - def ortho(left: Float, right: Float, bottom: Float, top: Float, near: Float, far: Float): Mat4 = { - val lr = 1 / (left - right) - val bt = 1 / (bottom - top) - val nf = 1 / (near - far) - - set(-2 * lr, 0, 0, 0, - 0, -2 * bt, 0, 0, - 0, 0, 2 * nf, 0, - (left + right) * lr, (top + bottom) * bt, (far + near) * nf, 1) - } - - def inverseTransposeInto(out: Mat3): Mat3 = { - val b00 = m00*m11 - m01*m10 - val b01 = m00*m12 - m02*m10 - val b02 = m00*m13 - m03*m10 - val b03 = m01*m12 - m02*m11 - val b04 = m01*m13 - m03*m11 - val b05 = m02*m13 - m03*m12 - val b06 = m20*m31 - m21*m30 - val b07 = m20*m32 - m22*m30 - val b08 = m20*m33 - m23*m30 - val b09 = m21*m32 - m22*m31 - val b10 = m21*m33 - m23*m31 - val b11 = m22*m33 - m23*m32 - - var det = b00*b11 - b01*b10 + b02*b09 + b03*b08 - b04*b07 + b05*b06 - if(det == 0) - return null - det = 1/det - - out.set((m11*b11 - m12*b10 + m13*b09)*det, - (m12*b08 - m10*b11 - m13*b07)*det, - (m10*b10 - m11*b08 + m13*b06)*det, - - (m02*b10 - m01*b11 - m03*b09)*det, - (m00*b11 - m02*b08 + m03*b07)*det, - (m01*b08 - m00*b10 - m03*b06)*det, - - (m31*b05 - m32*b04 + m33*b03)*det, - (m32*b02 - m30*b05 - m33*b01)*det, - (m30*b04 - m31*b02 + m33*b00)*det) - } - - override def toString: String = - s"Mat4($m00, $m01, $m02, $m03,\n" + - s" $m10, $m11, $m12, $m13,\n" + - s" $m20, $m21, $m22, $m23,\n" + - s" $m30, $m31, $m32, $m33)" - - def toArray(array: Array[Float]): Array[Float] = { - array(0) = m00; array(1) = m01; array(2) = m02; array(3) = m03 - array(4) = m10; array(5) = m11; array(6) = m12; array(7) = m13 - array(8) = m20; array(9) = m21; array(10) = m22; array(11) = m23 - array(12) = m30; array(13) = m31; array(14) = m32; array(15) = m33 - array - } -} - -object Mat4 { - def apply(): Mat4 = new Mat4() - - def apply(m00: Float, m01: Float, m02: Float, m03: Float, - m10: Float, m11: Float, m12: Float, m13: Float, - m20: Float, m21: Float, m22: Float, m23: Float, - m30: Float, m31: Float, m32: Float, m33: Float): Mat4 = - new Mat4(m00, m01, m02, m03, - m10, m11, m12, m13, - m20, m21, m22, m23, - m30, m31, m32, m33) - - def apply(m: Mat4): Mat4 = new Mat4(m) +package scryetek.vecmath + +/** + * Represents a 4x4 matrix of Floats. + */ +@inline +final class Mat4( + var m00: Float, var m01: Float, var m02: Float, var m03: Float, + var m10: Float, var m11: Float, var m12: Float, var m13: Float, + var m20: Float, var m21: Float, var m22: Float, var m23: Float, + var m30: Float, var m31: Float, var m32: Float, var m33: Float +) { + @inline + def set(m00: Float = m00, m01: Float = m01, m02: Float = m02, m03: Float = m03, + m10: Float = m10, m11: Float = m11, m12: Float = m12, m13: Float = m13, + m20: Float = m20, m21: Float = m21, m22: Float = m22, m23: Float = m23, + m30: Float = m30, m31: Float = m31, m32: Float = m33, m33: Float = m33): Mat4 = { + this.m00 = m00; this.m01 = m01; this.m02 = m02; this.m03 = m03 + this.m10 = m10; this.m11 = m11; this.m12 = m12; this.m13 = m13 + this.m20 = m20; this.m21 = m21; this.m22 = m22; this.m23 = m23 + this.m30 = m30; this.m31 = m31; this.m32 = m32; this.m33 = m33 + this + } + + @inline + def set(m: Mat4): Mat4 = + set(m.m00, m.m01, m.m02, m.m03, + m.m10, m.m11, m.m12, m.m13, + m.m20, m.m21, m.m22, m.m23, + m.m30, m.m31, m.m32, m.m33) + + @inline + def this() = + this(1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1) + + @inline + def transposed = + Mat4(m00, m10, m20, m30, + m01, m11, m21, m31, + m02, m12, m22, m32, + m03, m13, m23, m33) + + @inline + def transpose(out: Mat4 = this): Mat4 = + out.set(m00, m10, m20, m30, + m01, m11, m21, m31, + m02, m12, m22, m32, + m03, m13, m23, m33) + + @inline + def *(v: Vec4): Vec4 = + Vec4(m00 * v.x + m01 * v.y + m02 * v.z + m03 * v.w, + m10 * v.x + m11 * v.y + m12 * v.z + m13 * v.w, + m20 * v.x + m21 * v.y + m22 * v.z + m23 * v.w, + m30 * v.x + m31 * v.y + m32 * v.z + m33 * v.w) + + @inline + def *(q: Quat): Mat4 = { + val xx = q.x*q.x; val xy = q.x*q.y; val xz = q.x*q.z; val xw = q.x*q.w; val ww = q.w*q.w + val yy = q.y*q.y; val yz = q.y*q.z; val yw = q.y*q.w; val zz = q.z*q.z; val zw = q.z*q.w + + val b00 = 1-2*yy-2*zz; val b01 = 2*(xy-zw); val b02 = 2*(xz+yw); val b03 = 0 + val b10 = 2*(xy+zw); val b11 = 1-2*xx-2*zz; val b12 = 2*(yz-xw); val b13 = 0 + val b20 = 2*(xz-yw); val b21 = 2*(yz+xw); val b22 = 1-2*xx-2*yy; val b23 = 0 + val b30 = 0; val b31 = 0; val b32 = 0; val b33 = 1 + Mat4(b00 * m00 + b10 * m01 + b20 * m02 + b30 * m03, + b01 * m00 + b11 * m01 + b21 * m02 + b31 * m03, + b02 * m00 + b12 * m01 + b22 * m02 + b32 * m03, + b03 * m00 + b13 * m01 + b23 * m02 + b33 * m03, + + b00 * m10 + b10 * m11 + b20 * m12 + b30 * m13, + b01 * m10 + b11 * m11 + b21 * m12 + b31 * m13, + b02 * m10 + b12 * m11 + b22 * m12 + b32 * m13, + b03 * m10 + b13 * m11 + b23 * m12 + b33 * m13, + + b00 * m20 + b10 * m21 + b20 * m22 + b30 * m23, + b01 * m20 + b11 * m21 + b21 * m22 + b31 * m23, + b02 * m20 + b12 * m21 + b22 * m22 + b32 * m23, + b03 * m20 + b13 * m21 + b23 * m22 + b33 * m23, + + b00 * m30 + b10 * m31 + b20 * m32 + b30 * m33, + b01 * m30 + b11 * m31 + b21 * m32 + b31 * m33, + b02 * m30 + b12 * m31 + b22 * m32 + b32 * m33, + b03 * m30 + b13 * m31 + b23 * m32 + b33 * m33) + } + + @inline + def mul(v: Vec4, out: Vec4): Vec4 = + out.set(m00 * v.x + m01 * v.y + m02 * v.z + m03 * v.w, + m10 * v.x + m11 * v.y + m12 * v.z + m13 * v.w, + m20 * v.x + m21 * v.y + m22 * v.z + m23 * v.w, + m30 * v.x + m31 * v.y + m32 * v.z + m33 * v.w) + + @inline + def *(s: Float): Mat4 = + Mat4(m00*s, m01*s, m02*s, m03*s, + m10*s, m11*s, m12*s, m13*s, + m20*s, m21*s, m22*s, m23*s, + m30*s, m31*s, m32*s, m33*s) + + @inline + def scale(s: Float, out: Mat4 = this): Mat4 = + out.set(m00*s, m01*s, m02*s, m03*s, + m10*s, m11*s, m12*s, m13*s, + m20*s, m21*s, m22*s, m23*s, + m30*s, m31*s, m32*s, m33*s) + + @inline + def *(b: Mat4): Mat4 = + Mat4(b.m00 * m00 + b.m10 * m01 + b.m20 * m02 + b.m30 * m03, + b.m01 * m00 + b.m11 * m01 + b.m21 * m02 + b.m31 * m03, + b.m02 * m00 + b.m12 * m01 + b.m22 * m02 + b.m32 * m03, + b.m03 * m00 + b.m13 * m01 + b.m23 * m02 + b.m33 * m03, + + b.m00 * m10 + b.m10 * m11 + b.m20 * m12 + b.m30 * m13, + b.m01 * m10 + b.m11 * m11 + b.m21 * m12 + b.m31 * m13, + b.m02 * m10 + b.m12 * m11 + b.m22 * m12 + b.m32 * m13, + b.m03 * m10 + b.m13 * m11 + b.m23 * m12 + b.m33 * m13, + + b.m00 * m20 + b.m10 * m21 + b.m20 * m22 + b.m30 * m23, + b.m01 * m20 + b.m11 * m21 + b.m21 * m22 + b.m31 * m23, + b.m02 * m20 + b.m12 * m21 + b.m22 * m22 + b.m32 * m23, + b.m03 * m20 + b.m13 * m21 + b.m23 * m22 + b.m33 * m23, + + b.m00 * m30 + b.m10 * m31 + b.m20 * m32 + b.m30 * m33, + b.m01 * m30 + b.m11 * m31 + b.m21 * m32 + b.m31 * m33, + b.m02 * m30 + b.m12 * m31 + b.m22 * m32 + b.m32 * m33, + b.m03 * m30 + b.m13 * m31 + b.m23 * m32 + b.m33 * m33) + + @inline + def postMultiply(b: Mat4, out: Mat4): Mat4 = + postMultiply(b.m00, b.m01, b.m02, b.m03, + b.m10, b.m11, b.m12, b.m13, + b.m20, b.m21, b.m22, b.m23, + b.m30, b.m31, b.m32, b.m33, + out) + + @inline + def postMultiply(b: Mat4): Mat4 = + postMultiply(b.m00, b.m01, b.m02, b.m03, + b.m10, b.m11, b.m12, b.m13, + b.m20, b.m21, b.m22, b.m23, + b.m30, b.m31, b.m32, b.m33) + + @inline + def postMultiply(b00: Float, b01: Float, b02: Float, b03: Float, + b10: Float, b11: Float, b12: Float, b13: Float, + b20: Float, b21: Float, b22: Float, b23: Float, + b30: Float, b31: Float, b32: Float, b33: Float, + out: Mat4 = this): Mat4 = + out.set( + b00 * m00 + b10 * m01 + b20 * m02 + b30 * m03, + b01 * m00 + b11 * m01 + b21 * m02 + b31 * m03, + b02 * m00 + b12 * m01 + b22 * m02 + b32 * m03, + b03 * m00 + b13 * m01 + b23 * m02 + b33 * m03, + + b00 * m10 + b10 * m11 + b20 * m12 + b30 * m13, + b01 * m10 + b11 * m11 + b21 * m12 + b31 * m13, + b02 * m10 + b12 * m11 + b22 * m12 + b32 * m13, + b03 * m10 + b13 * m11 + b23 * m12 + b33 * m13, + + b00 * m20 + b10 * m21 + b20 * m22 + b30 * m23, + b01 * m20 + b11 * m21 + b21 * m22 + b31 * m23, + b02 * m20 + b12 * m21 + b22 * m22 + b32 * m23, + b03 * m20 + b13 * m21 + b23 * m22 + b33 * m23, + + b00 * m30 + b10 * m31 + b20 * m32 + b30 * m33, + b01 * m30 + b11 * m31 + b21 * m32 + b31 * m33, + b02 * m30 + b12 * m31 + b22 * m32 + b32 * m33, + b03 * m30 + b13 * m31 + b23 * m32 + b33 * m33 + ) + + @inline + def preMultiply(b: Mat4): Mat4 = + preMultiply(b.m00, b.m01, b.m02, b.m03, + b.m10, b.m11, b.m12, b.m13, + b.m20, b.m21, b.m22, b.m23, + b.m30, b.m31, b.m32, b.m33) + + @inline + def preMultiply(b: Mat4, out: Mat4): Mat4 = + preMultiply(b.m00, b.m01, b.m02, b.m03, + b.m10, b.m11, b.m12, b.m13, + b.m20, b.m21, b.m22, b.m23, + b.m30, b.m31, b.m32, b.m33, + out) + + /** + * Premultiplies the given matrix with this matrix, into the output matrix, non allocation version. + * allocations. + */ + @inline + def preMultiply( + b00: Float, b01: Float, b02: Float, b03: Float, + b10: Float, b11: Float, b12: Float, b13: Float, + b20: Float, b21: Float, b22: Float, b23: Float, + b30: Float, b31: Float, b32: Float, b33: Float, + out: Mat4 = this): Mat4 = + out.set(m00 * b00 + m10 * b01 + m20 * b02 + m30 * b03, + m01 * b00 + m11 * b01 + m21 * b02 + m31 * b03, + m02 * b00 + m12 * b01 + m22 * b02 + m32 * b03, + m03 * b00 + m13 * b01 + m23 * b02 + m33 * b03, + + m00 * b10 + m10 * b11 + m20 * b12 + m30 * b13, + m01 * b10 + m11 * b11 + m21 * b12 + m31 * b13, + m02 * b10 + m12 * b11 + m22 * b12 + m32 * b13, + m03 * b10 + m13 * b11 + m23 * b12 + m33 * b13, + + m00 * b20 + m10 * b21 + m20 * b22 + m30 * b23, + m01 * b20 + m11 * b21 + m21 * b22 + m31 * b23, + m02 * b20 + m12 * b21 + m22 * b22 + m32 * b23, + m03 * b20 + m13 * b21 + m23 * b22 + m33 * b23, + + m00 * b30 + m10 * b31 + m20 * b32 + m30 * b33, + m01 * b30 + m11 * b31 + m21 * b32 + m31 * b33, + m02 * b30 + m12 * b31 + m22 * b32 + m32 * b33, + m03 * b30 + m13 * b31 + m23 * b32 + m33 * b33) + + /** + * Destructively pre-multiplies a rotation around the axis into this matrix. + * @note the axis must be normalized. + */ + @inline + def preRotate(angle: Float, x: Float, y: Float, z: Float): Mat4 = { + val s = Math.sin(angle).toFloat + val c = Math.cos(angle).toFloat + val t = 1-c + + preMultiply(x*x*t + c, x*y*t - z*s, x*z*t+y*s, 0, + y*x*t + z*s, y*y*t + c, y*z*t-x*s, 0, + x*z*t - y*s, y*z*t + x*s, z*z*t + c, 0, + 0, 0, 0, 1) + } + + /** + * Destructively pre-multiplies a rotation around the axis into this matrix. + * @note the axis must be normalized. + */ + @inline + def preRotate(angle: Float, axis: Vec3): Mat4 = + preRotate(angle, axis.x, axis.y, axis.z) + + /** + * Destructively post-multiplies a rotation around the axis into this matrix. + * @note the axis must be normalized. + */ + @inline + def postRotate(angle: Float, x: Float, y: Float, z: Float): Mat4 = { + val s = Math.sin(angle).toFloat + val c = Math.cos(angle).toFloat + val t = 1-c + + postMultiply(x*x*t + c, x*y*t - z*s, x*z*t+y*s, 0, + y*x*t + z*s, y*y*t + c, y*z*t-x*s, 0, + x*z*t - y*s, y*z*t + x*s, z*z*t + c, 0, + 0, 0, 0, 1) + } + + /** + * Destructively post-multiplies a rotation around the axis into this matrix. + * @note the axis must be normalized. + */ + @inline + def postRotate(angle: Float, axis: Vec3): Mat4 = + postRotate(angle, axis.x, axis.y, axis.z) + + @inline + def preRotateQuat(x: Float, y: Float, z: Float, w: Float): Mat4 = { + val xx = x*x; val xy = x*y; val xz = x*z; val xw = x*w; val ww = w*w; + val yy = y*y; val yz = y*z; val yw = y*w; val zz = z*z; val zw = z*w; + preMultiply(1-2*yy-2*zz, 2*(xy-zw), 2*(xz+yw), 0, + 2*(xy+zw), 1-2*xx-2*zz, 2*(yz-xw), 0, + 2*(xz-yw), 2*(yz+xw), 1-2*xx-2*yy, 0, + 0, 0, 0, 1) + } + + @inline + def preRotateQuat(q: Quat): Mat4 = + preRotateQuat(q.x, q.y, q.z, q.w) + + @inline + def postRotateQuat(x: Float, y: Float, z: Float, w: Float): Mat4 = { + val xx = x*x; val xy = x*y; val xz = x*z; val xw = x*w; val ww = w*w; + val yy = y*y; val yz = y*z; val yw = y*w; val zz = z*z; val zw = z*w; + postMultiply(1-2*yy-2*zz, 2*(xy-zw), 2*(xz+yw), 0, + 2*(xy+zw), 1-2*xx-2*zz, 2*(yz-xw), 0, + 2*(xz-yw), 2*(yz+xw), 1-2*xx-2*yy, 0, + 0, 0, 0, 1) + } + + @inline + def postRotateQuat(q: Quat): Mat4 = + postRotateQuat(q.x, q.y, q.z, q.w) + + /** + * Destructively pre-multiplies a rotation around the x-axis into this matrix. + */ + @inline + def preRotateX(angle: Float): Mat4 = { + val s = Math.sin(angle).toFloat + val c = Math.cos(angle).toFloat + preMultiply(1, 0, 0, 0, + 0, c, -s, 0, + 0, s, c, 0, + 0, 0, 0, 1) + } + + /** + * Destructively post-multiplies a rotation around the x-axis into this matrix. + */ + @inline + def postRotateX(angle: Float): Mat4 = { + val s = Math.sin(angle).toFloat + val c = Math.cos(angle).toFloat + postMultiply(1, 0, 0, 0, + 0, c, -s, 0, + 0, s, c, 0, + 0, 0, 0, 1) + } + + /** + * Destructively pre-multiplies a rotation around the y-axis into this matrix. + */ + @inline + def preRotateY(angle: Float): Mat4 = { + val s = Math.sin(angle).toFloat + val c = Math.cos(angle).toFloat + preMultiply( c, 0, s, 0, + 0, 1, 0, 0, + -s, 0, c, 0, + 0, 0, 0, 1) + } + + /** + * Destructively post-multiplies a rotation around the y-axis into this matrix. + */ + @inline + def postRotateY(angle: Float): Mat4 = { + val s = Math.sin(angle).toFloat + val c = Math.cos(angle).toFloat + postMultiply( c, 0, s, 0, + 0, 1, 0, 0, + -s, 0, c, 0, + 0, 0, 0, 1) + } + + /** + * Destructively pre-multiplies a rotation around the z-axis into this matrix. + */ + @inline + def preRotateZ(angle: Float): Mat4 = { + val s = Math.sin(angle).toFloat + val c = Math.cos(angle).toFloat + preMultiply( c, -s, 0, 0, + s, c, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1) + } + + /** + * Destructively post-multiplies a rotation around the z-axis into this matrix. + */ + @inline + def postRotateZ(angle: Float): Mat4 = { + val s = Math.sin(angle).toFloat + val c = Math.cos(angle).toFloat + postMultiply( c, -s, 0, 0, + s, c, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1) + } + + @inline + def preScale(x: Float, y: Float, z: Float): Mat4 = + preMultiply(x, 0, 0, 0, + 0, y, 0, 0, + 0, 0, z, 0, + 0, 0, 0, 1) + @inline + def preScale(s: Vec3): Mat4 = + preScale(s.x, s.y, s.z) + + @inline + def postScale(x: Float, y: Float, z: Float): Mat4 = + postMultiply(x, 0, 0, 0, + 0, y, 0, 0, + 0, 0, z, 0, + 0, 0, 0, 1) + @inline + def postScale(s: Vec3): Mat4 = + postScale(s.x, s.y, s.z) + + @inline + def preTranslate(x: Float, y: Float, z: Float): Mat4 = + preMultiply(1, 0, 0, x, + 0, 1, 0, y, + 0, 0, 1, z, + 0, 0, 0, 1) + @inline + def preTranslate(t: Vec3): Mat4 = + preTranslate(t.x, t.y, t.z) + + @inline + def postTranslate(x: Float, y: Float, z: Float): Mat4 = + postMultiply(1, 0, 0, x, + 0, 1, 0, y, + 0, 0, 1, z, + 0, 0, 0, 1) + + @inline + def postTranslate(t: Vec3): Mat4 = + postTranslate(t.x, t.y, t.z) + + @inline + def +(b: Mat4): Mat4 = + Mat4(m00 + b.m00, m01 + b.m01, m02 + b.m02, m03 + b.m03, + m10 + b.m10, m11 + b.m11, m12 + b.m12, m13 + b.m13, + m20 + b.m20, m21 + b.m21, m22 + b.m22, m23 + b.m23, + m30 + b.m30, m31 + b.m31, m32 + b.m32, m33 + b.m33) + + @inline + def add(b: Mat4, out: Mat4 = this): Mat4 = + out.set(m00 + b.m00, m01 + b.m01, m02 + b.m02, m03 + b.m03, + m10 + b.m10, m11 + b.m11, m12 + b.m12, m13 + b.m13, + m20 + b.m20, m21 + b.m21, m22 + b.m22, m23 + b.m23, + m30 + b.m30, m31 + b.m31, m32 + b.m32, m33 + b.m33) + + + @inline + def -(b: Mat4): Mat4 = + Mat4(m00 - b.m00, m01 - b.m01, m02 - b.m02, m03 - b.m03, + m10 - b.m10, m11 - b.m11, m12 - b.m12, m13 - b.m13, + m20 - b.m20, m21 - b.m21, m22 - b.m22, m23 - b.m23, + m30 - b.m30, m31 - b.m31, m32 - b.m32, m33 - b.m33) + + @inline + def sub(b: Mat4, out: Mat4 = this): Mat4 = + out.set(m00 - b.m00, m01 - b.m01, m02 - b.m02, m03 - b.m03, + m10 - b.m10, m11 - b.m11, m12 - b.m12, m13 - b.m13, + m20 - b.m20, m21 - b.m21, m22 - b.m22, m23 - b.m23, + m30 - b.m30, m31 - b.m31, m32 - b.m32, m33 - b.m33) + + @inline + def determinant = { + val b00 = m00 * m11 - m01 * m10 + val b01 = m00 * m12 - m02 * m10 + val b02 = m00 * m13 - m03 * m10 + val b03 = m01 * m12 - m02 * m11 + val b04 = m01 * m13 - m03 * m11 + val b05 = m02 * m13 - m03 * m12 + val b06 = m20 * m31 - m21 * m30 + val b07 = m20 * m32 - m22 * m30 + val b08 = m20 * m33 - m23 * m30 + val b09 = m21 * m32 - m22 * m31 + val b10 = m21 * m33 - m23 * m31 + val b11 = m22 * m33 - m23 * m32 + + b00 * b11 - b01 * b10 + b02 * b09 + b03 * b08 - b04 * b07 + b05 * b06 + } + + @inline + def inverted = { + val b00 = m00 * m11 - m01 * m10 + val b01 = m00 * m12 - m02 * m10 + val b02 = m00 * m13 - m03 * m10 + val b03 = m01 * m12 - m02 * m11 + val b04 = m01 * m13 - m03 * m11 + val b05 = m02 * m13 - m03 * m12 + val b06 = m20 * m31 - m21 * m30 + val b07 = m20 * m32 - m22 * m30 + val b08 = m20 * m33 - m23 * m30 + val b09 = m21 * m32 - m22 * m31 + val b10 = m21 * m33 - m23 * m31 + val b11 = m22 * m33 - m23 * m32 + + var det = b00 * b11 - b01 * b10 + b02 * b09 + b03 * b08 - b04 * b07 + b05 * b06 + assert(det != 0, "Matrix is not invertable") + det = 1.0f / det + + Mat4((m11 * b11 - m12 * b10 + m13 * b09) * det, + (m02 * b10 - m01 * b11 - m03 * b09) * det, + (m31 * b05 - m32 * b04 + m33 * b03) * det, + (m22 * b04 - m21 * b05 - m23 * b03) * det, + + (m12 * b08 - m10 * b11 - m13 * b07) * det, + (m00 * b11 - m02 * b08 + m03 * b07) * det, + (m32 * b02 - m30 * b05 - m33 * b01) * det, + (m20 * b05 - m22 * b02 + m23 * b01) * det, + + (m10 * b10 - m11 * b08 + m13 * b06) * det, + (m01 * b08 - m00 * b10 - m03 * b06) * det, + (m30 * b04 - m31 * b02 + m33 * b00) * det, + (m21 * b02 - m20 * b04 - m23 * b00) * det, + + (m11 * b07 - m10 * b09 - m12 * b06) * det, + (m00 * b09 - m01 * b07 + m02 * b06) * det, + (m31 * b01 - m30 * b03 - m32 * b00) * det, + (m20 * b03 - m21 * b01 + m22 * b00) * det) + } + + def invert(out: Mat4 = this): Mat4 = { + val b00 = m00 * m11 - m01 * m10 + val b01 = m00 * m12 - m02 * m10 + val b02 = m00 * m13 - m03 * m10 + val b03 = m01 * m12 - m02 * m11 + val b04 = m01 * m13 - m03 * m11 + val b05 = m02 * m13 - m03 * m12 + val b06 = m20 * m31 - m21 * m30 + val b07 = m20 * m32 - m22 * m30 + val b08 = m20 * m33 - m23 * m30 + val b09 = m21 * m32 - m22 * m31 + val b10 = m21 * m33 - m23 * m31 + val b11 = m22 * m33 - m23 * m32 + + var det = b00 * b11 - b01 * b10 + b02 * b09 + b03 * b08 - b04 * b07 + b05 * b06 + assert(det != 0, "Matrix is not invertable") + det = 1.0f / det + + out.set((m11 * b11 - m12 * b10 + m13 * b09) * det, + (m02 * b10 - m01 * b11 - m03 * b09) * det, + (m31 * b05 - m32 * b04 + m33 * b03) * det, + (m22 * b04 - m21 * b05 - m23 * b03) * det, + + (m12 * b08 - m10 * b11 - m13 * b07) * det, + (m00 * b11 - m02 * b08 + m03 * b07) * det, + (m32 * b02 - m30 * b05 - m33 * b01) * det, + (m20 * b05 - m22 * b02 + m23 * b01) * det, + + (m10 * b10 - m11 * b08 + m13 * b06) * det, + (m01 * b08 - m00 * b10 - m03 * b06) * det, + (m30 * b04 - m31 * b02 + m33 * b00) * det, + (m21 * b02 - m20 * b04 - m23 * b00) * det, + + (m11 * b07 - m10 * b09 - m12 * b06) * det, + (m00 * b09 - m01 * b07 + m02 * b06) * det, + (m31 * b01 - m30 * b03 - m32 * b00) * det, + (m20 * b03 - m21 * b01 + m22 * b00) * det) + } + + /** Returns the adjoint of the matrix. */ + @inline + def adjointed = + Mat4( + m11 * (m22 * m33 - m23 * m32) - m21 * (m12 * m33 - m13 * m32) + m31 * (m12 * m23 - m13 * m22), + -(m01 * (m22 * m33 - m23 * m32) - m21 * (m02 * m33 - m03 * m32) + m31 * (m02 * m23 - m03 * m22)), + m01 * (m12 * m33 - m13 * m32) - m11 * (m02 * m33 - m03 * m32) + m31 * (m02 * m13 - m03 * m12), + -(m01 * (m12 * m23 - m13 * m22) - m11 * (m02 * m23 - m03 * m22) + m21 * (m02 * m13 - m03 * m12)), + -(m10 * (m22 * m33 - m23 * m32) - m20 * (m12 * m33 - m13 * m32) + m30 * (m12 * m23 - m13 * m22)), + m00 * (m22 * m33 - m23 * m32) - m20 * (m02 * m33 - m03 * m32) + m30 * (m02 * m23 - m03 * m22), + -(m00 * (m12 * m33 - m13 * m32) - m10 * (m02 * m33 - m03 * m32) + m30 * (m02 * m13 - m03 * m12)), + m00 * (m12 * m23 - m13 * m22) - m10 * (m02 * m23 - m03 * m22) + m20 * (m02 * m13 - m03 * m12), + m10 * (m21 * m33 - m23 * m31) - m20 * (m11 * m33 - m13 * m31) + m30 * (m11 * m23 - m13 * m21), + -(m00 * (m21 * m33 - m23 * m31) - m20 * (m01 * m33 - m03 * m31) + m30 * (m01 * m23 - m03 * m21)), + m00 * (m11 * m33 - m13 * m31) - m10 * (m01 * m33 - m03 * m31) + m30 * (m01 * m13 - m03 * m11), + -(m00 * (m11 * m23 - m13 * m21) - m10 * (m01 * m23 - m03 * m21) + m20 * (m01 * m13 - m03 * m11)), + -(m10 * (m21 * m32 - m22 * m31) - m20 * (m11 * m32 - m12 * m31) + m30 * (m11 * m22 - m12 * m21)), + m00 * (m21 * m32 - m22 * m31) - m20 * (m01 * m32 - m02 * m31) + m30 * (m01 * m22 - m02 * m21), + -(m00 * (m11 * m32 - m12 * m31) - m10 * (m01 * m32 - m02 * m31) + m30 * (m01 * m12 - m02 * m11)), + m00 * (m11 * m22 - m12 * m21) - m10 * (m01 * m22 - m02 * m21) + m20 * (m01 * m12 - m02 * m11)) + + /** Returns the adjoint of the matrix. */ + @inline + def adjointed(out: Mat4 = this): Mat4 = + out.set( + m11 * (m22 * m33 - m23 * m32) - m21 * (m12 * m33 - m13 * m32) + m31 * (m12 * m23 - m13 * m22), + -(m01 * (m22 * m33 - m23 * m32) - m21 * (m02 * m33 - m03 * m32) + m31 * (m02 * m23 - m03 * m22)), + m01 * (m12 * m33 - m13 * m32) - m11 * (m02 * m33 - m03 * m32) + m31 * (m02 * m13 - m03 * m12), + -(m01 * (m12 * m23 - m13 * m22) - m11 * (m02 * m23 - m03 * m22) + m21 * (m02 * m13 - m03 * m12)), + -(m10 * (m22 * m33 - m23 * m32) - m20 * (m12 * m33 - m13 * m32) + m30 * (m12 * m23 - m13 * m22)), + m00 * (m22 * m33 - m23 * m32) - m20 * (m02 * m33 - m03 * m32) + m30 * (m02 * m23 - m03 * m22), + -(m00 * (m12 * m33 - m13 * m32) - m10 * (m02 * m33 - m03 * m32) + m30 * (m02 * m13 - m03 * m12)), + m00 * (m12 * m23 - m13 * m22) - m10 * (m02 * m23 - m03 * m22) + m20 * (m02 * m13 - m03 * m12), + m10 * (m21 * m33 - m23 * m31) - m20 * (m11 * m33 - m13 * m31) + m30 * (m11 * m23 - m13 * m21), + -(m00 * (m21 * m33 - m23 * m31) - m20 * (m01 * m33 - m03 * m31) + m30 * (m01 * m23 - m03 * m21)), + m00 * (m11 * m33 - m13 * m31) - m10 * (m01 * m33 - m03 * m31) + m30 * (m01 * m13 - m03 * m11), + -(m00 * (m11 * m23 - m13 * m21) - m10 * (m01 * m23 - m03 * m21) + m20 * (m01 * m13 - m03 * m11)), + -(m10 * (m21 * m32 - m22 * m31) - m20 * (m11 * m32 - m12 * m31) + m30 * (m11 * m22 - m12 * m21)), + m00 * (m21 * m32 - m22 * m31) - m20 * (m01 * m32 - m02 * m31) + m30 * (m01 * m22 - m02 * m21), + -(m00 * (m11 * m32 - m12 * m31) - m10 * (m01 * m32 - m02 * m31) + m30 * (m01 * m12 - m02 * m11)), + m00 * (m11 * m22 - m12 * m21) - m10 * (m01 * m22 - m02 * m21) + m20 * (m01 * m12 - m02 * m11)) + + @inline + def toAngleAxis: AngleAxis = { + // Shamelessly cribbed and ported from the java version at + // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/ + + val epsilon = 0.01f; // margin to allow for rounding errors + val epsilon2 = 0.1f; // margin to distinguish between 0 and 180 degrees + if (math.abs(m01 - m10) < epsilon && math.abs(m02-m20) < epsilon && math.abs(m12-m21) < epsilon) { + // singularity found + // first check for identity matrix which must have +1 for all terms + // in leading diagonal and zero in other terms + if (math.abs(m01 + m10) < epsilon2 && math.abs(m02+m20) < epsilon2 && + math.abs(m12+m21) < epsilon2 && math.abs(m00 + m11 + m22 - 3) < epsilon2) { + // this singularity is identity matrix so angle = 0 + return AngleAxis(0,Vec3(1,0,0)); // zero angle, arbitrary axis + } + // otherwise this singularity is angle = 180 + val angle = Math.PI.toFloat + val xx = (m00 + 1) / 2 + val yy = (m11 + 1) / 2 + val zz = (m22 + 1) / 2 + val xy = (m01 + m10) / 4 + val xz = (m02 + m20) / 4 + val yz = (m12 + m21) / 4 + if (xx > yy && xx > zz) { + // m00 is the largest diagonal term + if (xx < epsilon) + return AngleAxis(angle, Vec3(0, 0.7071f, 0.7071f)) + else { + val x = Math.sqrt(xx).toFloat + return AngleAxis(angle, Vec3(x, xy / x, xz / x)) + } + } else if (yy > zz) { + // m11 is the largest diagonal term + if (yy < epsilon) + return AngleAxis(angle, Vec3(0.7071f, 0, 0.7071f)) + else { + val y = Math.sqrt(yy).toFloat + return AngleAxis(angle, Vec3(xy / y, y, yz / y)) + } + } else { + // m22 is the largest diagonal term so base result on this + if (zz < epsilon) { + return AngleAxis(angle, Vec3(0.7071f, 0.7071f, 0)) + } else { + val z = Math.sqrt(zz).toFloat + return AngleAxis(angle, Vec3(xz / z, yz / z, z)) + } + } + } + // as we have reached here there are no singularities so we can handle normally + var s = math.sqrt((m21 - m12)*(m21 - m12) + (m02 - m20)*(m02 - m20) + (m10 - m01)*(m10 - m01)).toFloat // used to normalise + if (Math.abs(s) < 0.001) s=1 + // prevent divide by zero, should not happen if matrix is orthogonal and should be + // caught by singularity test above, but I've left it in just in case + AngleAxis(Math.acos((m00 + m11 + m22 - 1)/2).toFloat, Vec3((m21 - m12)/s, (m02 - m20)/s, (m10 - m01)/s)) + } + + @inline + def toQuat = { + val tr = m00 + m11 + m22 + if (tr > 0) { + val S = math.sqrt(tr + 1.0f).toFloat * 2 // S=4*qw + Quat((m21 - m12) / S, (m02 - m20) / S, (m10 - m01) / S, 0.25f * S).normalized + } else if ((m00 > m11) & (m00 > m22)) { + val S = math.sqrt(1.0 + m00 - m11 - m22).toFloat * 2 // S=4*qx + Quat(0.25f * S, (m01 + m10) / S, (m02 + m20) / S,(m21 - m12) / S).normalized + } else if (m11 > m22) { + val S = math.sqrt(1.0 + m11 - m00 - m22).toFloat * 2 // S=4*qy + Quat((m01 + m10) / S, 0.25f * S, (m12 + m21) / S, (m02 - m20) / S).normalized + } else { + val S = math.sqrt(1.0 + m22 - m00 - m11).toFloat * 2; // S=4*qz + Quat((m02 + m20) / S, (m12 + m21) / S, 0.25f * S, (m10 - m01) / S).normalized + } + } + + def copy(m00: Float = m00, m01: Float = m01, m02: Float = m02, m03: Float = m03, + m10: Float = m10, m11: Float = m11, m12: Float = m12, m13: Float = m13, + m20: Float = m20, m21: Float = m21, m22: Float = m22, m23: Float = m23, + m30: Float = m30, m31: Float = m31, m32: Float = m33, m33: Float = m33): Mat4 = + set(m00, m01, m02, m03, + m10, m11, m12, m13, + m20, m21, m22, m23, + m30, m31, m32, m33) + + override def toString = + s"Mat4(${m00}f,${m01}f,${m02}f,${m03}f, ${m10}f,${m11}f,${m12}f,${m13}f,${m20}f,${m21}f,${m22}f,${m23}f)" + + override def equals(o: Any): Boolean = o match { + case m: Mat4 => + m00 == m.m00 && m01 == m.m01 && m02 == m.m02 && m03 == m.m03 && + m10 == m.m10 && m11 == m.m11 && m12 == m.m12 && m13 == m.m13 && + m20 == m.m20 && m21 == m.m21 && m22 == m.m22 && m23 == m.m23 && + m30 == m.m30 && m31 == m.m31 && m32 == m.m32 && m33 == m.m33 + case _ => false + } + + override def hashCode: Int = + m00.hashCode()*19 + m01.hashCode()*23 + m02.hashCode()*29 + m03.hashCode()*31 + + m10.hashCode()*37 + m11.hashCode()*41 + m12.hashCode()*43 + m13.hashCode()*47 + + m20.hashCode()*53 + m21.hashCode()*59 + m22.hashCode()*61 + m22.hashCode()*67 + + m30.hashCode()*71 + m31.hashCode()*73 + m32.hashCode()*79 + m32.hashCode()*83 +} + +object Mat4 { + def apply(m00: Float, m01: Float, m02: Float, m03: Float, + m10: Float, m11: Float, m12: Float, m13: Float, + m20: Float, m21: Float, m22: Float, m23: Float, + m30: Float, m31: Float, m32: Float, m33: Float): Mat4 = + new Mat4(m00, m01, m02, m03, + m10, m11, m12, m13, + m20, m21, m22, m23, + m30, m31, m32, m33) + + def apply(): Mat4 = + new Mat4() + + @inline + def rotateQuat(q: Quat): Mat4 = + rotateQuat(q.x, q.y, q.z, q.w) + + @inline + def rotateQuat(x: Float, y: Float, z: Float, w: Float): Mat4 = { + val xx = x*x; val xy = x*y; val xz = x*z; val xw = x*w; val ww = w*w; + val yy = y*y; val yz = y*z; val yw = y*w; val zz = z*z; val zw = z*w; + Mat4( + 1-2*yy-2*zz, 2*(xy-zw), 2*(xz+yw), 0, + 2*(xy+zw), 1-2*xx-2*zz, 2*(yz-xw), 0, + 2*(xz-yw), 2*(yz+xw), 1-2*xx-2*yy, 0, + 0, 0, 0, 1) + } + + /** + * Returns the rotation matrix about the given angle and axis. + * @param angle the angle to rotate, in radians. + * @param x the x-component of the axis vector to rotate around, must be normalized. + * @param y the y-component of the axis vector to rotate around, must be normalized. + * @param z the z-component of the axis vector to rotate around, must be normalized. + */ + @inline + def rotate(angle: Float, x: Float, y: Float, z: Float): Mat4 = { + val s = Math.sin(angle).toFloat + val c = Math.cos(angle).toFloat + val t = 1-c + + Mat4(x*x*t + c, x*y*t - z*s, x*z*t+y*s, 0, + y*x*t + z*s, y*y*t + c, y*z*t-x*s, 0, + x*z*t - y*s, y*z*t + x*s, z*z*t + c, 0, + 0, 0, 0, 1) + } + + /** + * Returns the rotation matrix about the given angle and axis. + * @param angle the angle to rotate, in radians. + * @param axis the axis vector to rotate around, must be normalized. + */ + @inline + def rotate(angle: Float, axis: Vec3): Mat4 = + rotate(angle, axis.x, axis.y, axis.z) + + /** + * Returns the rotation matrix rotating around the X-axis. + */ + @inline + def rotateX(angle: Float): Mat4 = { + val s = Math.sin(angle).toFloat + val c = Math.cos(angle).toFloat + Mat4(1, 0, 0, 0, + 0, c, -s, 0, + 0, s, c, 0, + 0, 0, 0, 1) + } + + /** + * Returns the rotation matrix rotating around the Y-axis. + */ + @inline + def rotateY(angle: Float): Mat4 = { + val s = Math.sin(angle).toFloat + val c = Math.cos(angle).toFloat + Mat4( c, 0, s, 0, + 0, 1, 0, 0, + -s, 0, c, 0, + 0, 0, 0, 1) + } + + /** + * Returns the rotation matrix rotating around the Z-axis. + */ + @inline + def rotateZ(angle: Float): Mat4 = { + val s = Math.sin(angle).toFloat + val c = Math.cos(angle).toFloat + Mat4( c, -s, 0, 0, + s, c, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1) + } + + /** + * Returns a scale matrix + * @param x the x-scale + * @param y the y-scale + * @param z the z-scale + */ + @inline + def scale(x: Float, y: Float, z: Float): Mat4 = + Mat4(x, 0, 0, 0, + 0, y, 0, 0, + 0, 0, z, 0, + 0, 0, 0, 1) + + /** + * Returns a scale matrix + * @param s the scale vector + */ + @inline + def scale(s: Vec3): Mat4 = + scale(s.x, s.y, s.z) + + /** + * Returns a translate matrix + * @param x the x-translation + * @param y the y-translation + * @param z the z-translation + */ + @inline + def translate(x: Float, y: Float, z: Float): Mat4 = + Mat4(1, 0, 0, x, + 0, 1, 0, y, + 0, 0, 1, z, + 0, 0, 0, 1) + + /** + * Returns a translate matrix + * @param t the translation vector + */ + @inline + def translate(t: Vec3): Mat4 = + translate(t.x, t.y, t.z) + + @inline + def frustum(left: Float, right: Float, bottom: Float, top: Float, zNear: Float, zFar: Float): Mat4 = { + val A = (right + left)/(right - left) + val B = (top + bottom)/(top - bottom) + val C = (zFar + zNear)/(zFar - zNear) + val D = (2*zFar*zNear)/(zFar - zNear) + Mat4((2*zNear*zFar)/(right - left), 0, A, 0, + 0, (2*zNear)/(top - bottom), B, 0, + 0, 0, C, D, + 0, 0, -1, 0) + } + + /** + * Construct a perspective projection matrix + * @param fovy the fov of the y, in radians. + * @param aspect the aspect ratio of the viewport + * @param zNear the near plane position + * @param zFar the far plane position + * @note as the near plane approaches 0, depth buffer precision approaches 0. + */ + @inline + def perspective(fovy: Float, aspect: Float, zNear: Float, zFar: Float): Mat4 = { + val f = 1.0f / Math.tan(fovy / 2).toFloat + val nf = 1 / (zNear-zFar) + + Mat4(f/aspect, 0, 0, 0, + 0, f, 0, 0, + 0, 0, (zFar + zNear)*nf, 2*(zFar*zNear)*nf, + 0, 0, -1, 0) + } + + /** + * Constructs an orthographic projection martix + * @param left the left plane position + * @param right the right plane position + * @param bottom the bottom plane position + * @param top the top plane position + * @param zNear the near plane position + * @param zFar the far plane position + */ + @inline + def ortho(left: Float, right: Float, bottom: Float, top: Float, zNear: Float = -1, zFar: Float = 1): Mat4 = { + val tx = -(right + left)/(right - left) + val ty = -(top + bottom)/(top - bottom) + val tz = -(zFar + zNear)/(zFar - zNear) + + Mat4(2/(right-left), 0, 0, tx, + 0, 2/(top-bottom), 0, ty, + 0, 0, -2/(zFar - zNear), tz, + 0, 0, 0, 1) + } + + /** + * Constructs a rotation and trnslation matrix that positions at the eye vector, + * looking at the center vector, with the given up vector. + * + * None of these vectors need normalization. + * + * @param eye the eye vector - the position we are at. + * @param center the center vector - the point we are looking at. + * @param up the up vector - which direction points up. + */ + @inline + def lookAt(eye: Vec3, center: Vec3, up: Vec3): Mat4 = { + val f = (center - eye).normalized + val s = f ⨯ up.normalized + val u = s.normalized ⨯ f + Mat4( s.x, s.y, s.z, 0, + u.x, u.y, u.z, 0, + -f.x, -f.y, -f.z, 0, + 0, 0, 0, 1) * translate(-eye) + } } \ No newline at end of file diff --git a/shared/src/main/scala/scryetek/vecmath/Quat.scala b/shared/src/main/scala/scryetek/vecmath/Quat.scala index 3ae5b75..d49df49 100644 --- a/shared/src/main/scala/scryetek/vecmath/Quat.scala +++ b/shared/src/main/scala/scryetek/vecmath/Quat.scala @@ -1,181 +1,257 @@ -package scryetek.vecmath - -/** - * Created by Matt on 29/06/2014. - */ -class Quat(var x: Float, var y: Float, var z: Float, var w: Float) { - def this() = this(0, 0, 0, 1) - - def set(x: Float, y: Float, z: Float, w: Float): Quat = { - this.x = x; this.y = y; this.z = z; this.w = w - this - } - - def set(q: Quat): Quat = - set(q.x, q.y, q.z, q.w) - - - def add(q: Quat): Quat = { - x += q.x; y += q.y; z += q.z; w += q.w - this - } - - // rotationTo - // setAxis - def setIdentity: Quat = - set(0, 0, 0, 1) - - def setAngleAxis(angle: Float, axis: Vec3): Quat = { - val halfAngle = angle * 0.5f - val s = Math.sin(halfAngle).toFloat - set(s*x, s*y, s*z, Math.cos(halfAngle).toFloat) - } - - def mulInto(out: Quat, q: Quat): Quat = - out.set(x*q.w + w*q.x + y*q.z - z*q.y, - y*q.w + w*q.y + z*q.x - x*q.z, - z*q.w + w*q.z + x*q.y - y*q.x, - w*q.w - x*q.x - y*q.y - z*q.z) - - def mul(q: Quat): Quat = - mulInto(this, q) - - def rotateXInto(out: Quat, angle: Float): Quat = { - val halfAngle = angle*0.5f - val bx = Math.sin(halfAngle).toFloat - val bw = Math.cos(halfAngle).toFloat - - out.set(x*bw + w*bx, y*bw + z*bx, z*bw - y*bx, w*w - x*bx) - } - - def rotateX(angle: Float): Quat = - rotateXInto(this, angle) - - def rotateYInto(out: Quat, angle: Float): Quat = { - val halfAngle = angle*0.5f - val by = Math.sin(halfAngle).toFloat - val bw = Math.cos(halfAngle).toFloat - - set(x*bw - z*by, y*bw + w*by, z*bw + x*by, w*bw - y*by) - } - - def rotateY(angle: Float): Quat = - rotateYInto(this, angle) - - def rotateZInto(out: Quat, angle: Float): Quat = { - val halfAngle = angle*0.5f - val bz = Math.sin(halfAngle).toFloat - val bw = Math.cos(halfAngle).toFloat - - out.set(x*bw + y*bz, y*bw - x*bz, z*bw + w*bz, w*bw - z*bz) - } - - def rotateZ(angle: Float): Quat = - rotateZInto(this, angle) - - // re-normalizes this quaternion - def calculateWInto(out: Quat): Quat = - out.set(x, y, z, -Math.sqrt(Math.abs(1-x*x - y*y - z*z)).toFloat) - - def calculateW: Quat = - calculateWInto(this) - - def dot(q: Quat): Float = - x*q.x + y*q.y + z*q.z + w*q.w - - // lerp - def slerpInto(out: Quat, b: Quat, t: Float): Quat = { - var bx = b.x; var by = b.y; var bz = b.z; var bw = b.w - val cosom = dot(b) - var scale0 = 0.0f; var scale1 = 0.0f - if(cosom < 0) { - bx = -bx - by = -by - bz = -bz - bw = -bw - } - if(1.0f - cosom > 0.0001) { - val omega = Math.acos(cosom).toFloat - val sinom = Math.sin(omega).toFloat - scale0 = Math.sin((1.0-t)*omega).toFloat/sinom - scale1 = Math.sin(t*omega).toFloat/sinom - } else { - scale0 = 1.0f - t - scale1 = t - } - - out.set(scale0*x + scale1*bx, - scale0*y + scale1*by, - scale0*z + scale1*bz, - scale0*w + scale1*bw) - } - - def slerp(b: Quat, t: Float): Quat = - slerpInto(this, b, t) - - def invertInto(out: Quat): Quat = { - val dot = this.dot(this) - val invDot = if(dot != 0) 1/dot else 0 - out.set(-x*invDot, -y*invDot, -z*invDot, w*invDot) - } - - def invert: Quat = - invertInto(this) - - def conjugateInto(q: Quat): Quat = - q.set(-x, -y, -z, w) - - def conjugate: Quat = - this.conjugateInto(this) - - def magnitude: Float = - Math.sqrt(magnitude).toFloat - - def magSqr: Float = - x*x + y*y + z*z + w*w - - def scale(s: Float): Quat = { - x *= s; y *= s; z *= s; w*= s - this - } - - def normalize: Quat = - scale(magnitude) - - // fromMat3 - override def toString = s"Quat($x, $y, $z, $w)" - - def toEuler: Vec3 = { - val sqw = w*w - val sqx = x*x - val sqy = y*y - val sqz = z*z - val unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor - val test = x*y + z*w - if (test > 0.499*unit) // singularity at north pole - Vec3(2 * math.atan2(x,w).toFloat, Math.PI.toFloat/2, 0) - else if (test < -0.499*unit) // singularity at south pole - Vec3(-2 * math.atan2(x,w).toFloat, -Math.PI.toFloat/2, 0) - else - Vec3(math.atan2(2*y*w-2*x*z , sqx - sqy - sqz + sqw).toFloat, - math.asin(2*test/unit).toFloat, - math.atan2(2*x*w-2*y*z , -sqx + sqy - sqz + sqw).toFloat) - } -} - -object Quat { - def fromEuler(x: Float, y: Float, z: Float): Quat = { - val sinX = math.sin(x*0.5f).toFloat - val cosX = math.cos(x*0.5f).toFloat - val sinY = math.sin(y*0.5f).toFloat - val cosY = math.cos(y*0.5f).toFloat - val sinZ = math.sin(z*0.5f).toFloat - val cosZ = math.cos(z*0.5f).toFloat - val cosXY = cosX*cosY - val sinXY = sinX*sinY - new Quat(sinZ*cosXY - cosZ*sinXY, - cosZ*sinX*cosY + sinZ*cosX*sinY, - cosZ*cosX*sinY - sinZ*sinX*cosY, - cosZ*cosXY + sinZ*sinXY) - } -} \ No newline at end of file +package scryetek.vecmath + +@inline +final class Quat(var x: Float, var y: Float, var z: Float, var w: Float) { + @inline + def set(x: Float = this.x, y: Float = this.y, z: Float = this.z, w: Float = this.w) = { + this.x = x + this.y = y + this.z = z + this.w = w + this + } + + @inline + def set(q: Quat): Quat = + set(q.x, q.y, q.z, q.w) + + @inline + def +(q: Quat): Quat = + Quat(x + q.x, y + q.y, z + q.z, w + q.w) + + @inline + def add(q: Quat, out: Quat = this): Quat = + out.set(x + q.x, y + q.y, z + q.z, w + q.w) + + @inline + def add(x: Float, y: Float, z: Float, w: Float, out: Quat): Quat = + out.set(this.x + x, this.y + y, this.z + z, this.w + w) + + @inline + def add(x: Float, y: Float, z: Float, w: Float): Quat = + add(x, y, z, w, this) + + @inline + def -(q: Quat): Quat = + Quat(x - q.x, y - q.y, z - q.z, w - q.w) + + + @inline + def sub(q: Quat, out: Quat = this): Quat = + out.set(x - q.x, y - q.y, z - q.z, w - q.w) + + @inline + def sub(x: Float, y: Float, z: Float, w: Float, out: Quat): Quat = + out.set(this.x - x, this.y - y, this.z - z, this.w - w) + + @inline + def sub(x: Float, y: Float, z: Float, w: Float): Quat = + sub(x, y, z, w, this) + + @inline + def *(q: Quat): Quat = + Quat(x*q.w + w*q.x + y*q.z - z*q.y, + y*q.w + w*q.y + z*q.x - x*q.z, + z*q.w + w*q.z + x*q.y - y*q.x, + w*q.w - x*q.x - y*q.y - z*q.z) + + @inline + def postMultiply(q: Quat, out: Quat = this): Quat = + out.set(x*q.w + w*q.x + y*q.z - z*q.y, + y*q.w + w*q.y + z*q.x - x*q.z, + z*q.w + w*q.z + x*q.y - y*q.x, + w*q.w - x*q.x - y*q.y - z*q.z) + + @inline + def *(v: Vec4): Vec4 = + (this * Quat(v.x, v.y, v.z, v.w) * this.conjugated).toVec4 + + @inline + def *(v: Vec3): Vec3 = { + val ox = x + w*v.x + y*v.z - z*v.y + val oy = y + w*v.y + z*v.x - x*v.z + val oz = z + w*v.z + x*v.y - y*v.x + val ow = w - x*v.x - y*v.y - z*v.z + + val s = 1/(ow*w - ox * -x - oy * -y - oz * -z) + Vec3((ox*w + ow * -x + oy * -z - oz * -y)*s, + (oy*w + ow * -y + oz * -x - ox * -z)*s, + (oz*w + ow * -z + ox * -y - oy * -x)*s) + } + + @inline + def mul(v: Vec4, out: Vec4) = { + val ox = x*v.w + w*v.x + y*v.z - z*v.y + val oy = y*v.w + w*v.y + z*v.x - x*v.z + val oz = z*v.w + w*v.z + x*v.y - y*v.x + val ow = w*v.w - x*v.x - y*v.y - z*v.z + + out.set(ox*w + ow * -x + oy * -z - oz * -y, + oy*w + ow * -y + oz * -x - ox * -z, + oz*w + ow * -z + ox * -y - oy * -x, + ow*w - ox * -x - oy * -y - oz * -z) + } + + def mul(v: Vec4): Vec4 = + mul(v, v) + + @inline + def mul(v: Vec3, out: Vec3) = { + val ox = x + w*v.x + y*v.z - z*v.y + val oy = y + w*v.y + z*v.x - x*v.z + val oz = z + w*v.z + x*v.y - y*v.x + val ow = w - x*v.x - y*v.y - z*v.z + + val s = 1/(ow*w - ox * -x - oy * -y - oz * -z) + out.set((ox*w + ow * -x + oy * -z - oz * -y)*s, + (oy*w + ow * -y + oz * -x - ox * -z)*s, + (oz*w + ow * -z + ox * -y - oy * -x)*s) + } + + + + @inline + def *(s: Float): Quat = + Quat(x*s, y*s, z*s, w*s) + + @inline + def /(s: Float): Quat = + this*(1/s) + + @inline + def dot(q: Quat): Float = + x*q.x + y*q.y + z*q.z + w*q.w + + @inline + def toAngleAxis: AngleAxis = { + val s = math.sqrt(1-w*w).toFloat + if(s < 0.001) { + // angle is very small and we risk dividing by zero. return identity rotation instead. + AngleAxis(0, Vec3(1, 0, 0)) + } else { + val s2 = 1/s + AngleAxis(2 * math.acos(w).toFloat, Vec3(x * s2, y * s2, z * s2)) + } + } + + /** + * Converts this (normalized) quaternion into a matrix. + */ + @inline + def toMat4 = { + val xx = x*x; val xy = x*y; val xz = x*z; val xw = x*w; val ww = w*w; + val yy = y*y; val yz = y*z; val yw = y*w; val zz = z*z; val zw = z*w; + Mat4( + 1-2*yy-2*zz, 2*(xy-zw), 2*(xz+yw), 0, + 2*(xy+zw), 1-2*xx-2*zz, 2*(yz-xw), 0, + 2*(xz-yw), 2*(yz+xw), 1-2*xx-2*yy, 0, + 0, 0, 0, 1) + } + + /** + * Converts this (normalized) quaternion into a matrix. + */ + @inline + def toMat3 = { + val xx = x*x; val xy = x*y; val xz = x*z; val xw = x*w; val ww = w*w; + val yy = y*y; val yz = y*z; val yw = y*w; val zz = z*z; val zw = z*w; + Mat3( + 1-2*yy-2*zz, 2*(xy-zw), 2*(xz+yw), + 2*(xy+zw), 1-2*xx-2*zz, 2*(yz-xw), + 2*(xz-yw), 2*(yz+xw), 1-2*xx-2*yy) + } + + @inline + def conjugated = + Quat(-x, -y, -z, w) + + @inline + def toVec4 = + Vec4(x, y, z, w) + + @inline + def magSqr = x*x + y*y + z*z + w*w + + @inline + def magnitude = math.sqrt(magSqr).toFloat + + @inline + def normalized: Quat = + this / magnitude + + @inline + def lerp(q: Quat, t: Float): Quat = + Quat(x + t*(q.x-x), + y + t*(q.y-y), + z + t*(q.z-z), + w + t*(q.w-w)) + + @inline + def lerp(q: Quat, t: Float, out: Quat): Quat = + out.set(x + t*(q.x-x), + y + t*(q.y-y), + z + t*(q.z-z), + w + t*(q.w-w)) + + def slerp(q: Quat, t: Float): Quat = + slerp(q, t, Quat()) + + def slerp(q: Quat, t: Float, out: Quat): Quat = { + // another shameless crib from http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp + var qx = q.x; var qy = q.y; var qz = q.z; var qw = q.w + // Calculate angle between them. + var cosHalfTheta = w*qw + x*qx + y*qy + z*qz + if(cosHalfTheta < 0) { + qw = -qw; qx = -qx; qy = -qy; qz = qz + cosHalfTheta = -cosHalfTheta + } + // if qa=qb or qa=-qb then theta = 0 and we can return qa + if (math.abs(cosHalfTheta) >= 1.0) + out.set(x, y, z, w) + else { + // Calculate temporary values. + val halfTheta = math.acos(cosHalfTheta).toFloat + val sinHalfTheta = math.sqrt(1.0 - cosHalfTheta * cosHalfTheta).toFloat + // if theta = 180 degrees then result is not fully defined + // we could rotate around any axis normal to qa or qb + if (math.abs(sinHalfTheta) < 0.001f) + out.set(x*0.5f + qx*0.5f, y*0.5f + qy*0.5f, z*0.5f + qz*0.5f, w*0.5f + qw*0.5f) + else { + val ratioA = math.sin((1 - t) * halfTheta).toFloat / sinHalfTheta + val ratioB = math.sin(t * halfTheta).toFloat / sinHalfTheta + //calculate Quaternion. + out.set(x * ratioA + qx * ratioB, y * ratioA + qy * ratioB, z * ratioA + qz * ratioB, w * ratioA + qw * ratioB) + } + } + } + + def copy(x: Float = x, y: Float = y, z: Float = z, w: Float = w) = + Quat(x,y,z,w) + + override def toString = + s"Quat(${x}f,${y}f,${z}f,${w}f)" + + override def equals(o: Any): Boolean = o match { + case v: Quat => x == v.x && y == v.y && z == v.z && w == v.w + case _ => false + } + + override def hashCode: Int = + x.hashCode() * 31 + y.hashCode() * 53 + z.hashCode() + w.hashCode() * 71 +} + +object Quat { + def apply() = + new Quat(0, 0, 0, 1) + + def apply(x: Float, y: Float, z: Float, w: Float) = + new Quat(x, y, z, w) + + def fromAngleAxis(angle: Float, x: Float, y: Float, z: Float): Quat = { + val s = math.sin(angle/2).toFloat + Quat(x*s, y*s, z*s, math.cos(angle/2).toFloat) + } + + def fromAngleAxis(angle: Float, axis: Vec3): Quat = + fromAngleAxis(angle, axis.x, axis.y, axis.z) +} diff --git a/shared/src/main/scala/scryetek/vecmath/Vec2.scala b/shared/src/main/scala/scryetek/vecmath/Vec2.scala index 234d136..7544d38 100644 --- a/shared/src/main/scala/scryetek/vecmath/Vec2.scala +++ b/shared/src/main/scala/scryetek/vecmath/Vec2.scala @@ -1,74 +1,185 @@ -package scryetek.vecmath - -/** - * Created by Matt on 05/07/2014. - */ -case class Vec2(var x: Float, var y: Float) { - def this() = this(0, 0) - def this(v: Vec2) = this(v.x, v.y) - - def set(x: Float, y: Float): Vec2 = { - this.x = x - this.y = y - this - } - - def set(v: Vec2): Vec2 = { - this.x = v.x - this.y = v.y - this - } - - def +(o: Vec2): Vec2 = Vec2(x+o.x, y+o.y) - def -(o: Vec2): Vec2 = Vec2(x-o.x, y-o.y) - def *(o: Vec2): Float = x*o.x + y*o.y - def *(s: Float): Vec2 = Vec2(x * s, y*s) - def /(s: Float): Vec2 = this * (1/s) - - def unary_- : Vec2 = Vec2(-x, -y) - - def add(v: Vec2): Vec2 = { - this.x += v.x - this.y += v.y - this - } - - def sub(v: Vec2): Vec2 = { - this.x -= v.x - this.y -= v.y - this - } - - def scale(s: Float): Vec2 = { - x *= s; y *= s - this - } - - def divide(s: Float): Vec2 = scale(1/s) - - def magSqr: Float = x*x + y*y - def magnitude: Float = Math.sqrt(x*x + y*y).toFloat - - def normalize = - scale(1/magnitude) - - def negate: Vec2 = { - this.x = -x - this.y = -y - this - } - - def dot(v: Vec2) = x*v.x + y*v.y - - def zNormal(v: Vec2) = x*v.y - y*v.x - - def max(v: Vec2) = Vec2(x max v.x, y max v.y) - def min(v: Vec2) = Vec2(x min v.x, y min v.y) - - override def toString: String = s"Vec2($x, $y)" -} - -object Vec2 { - def apply() = new Vec2() - def apply(v: Vec2) = new Vec2(v) +package scryetek.vecmath + +@inline +final class Vec2(var x: Float, var y: Float) { + @inline + def this() = this(0, 0) + + @inline + def set(v: Vec2): Vec2 = + set(v.x, v.y) + + @inline + def set(x: Float = this.x, y: Float = this.y): Vec2 = { + this.x = x + this.y = y + this + } + + /** Adds two vectors. */ + @inline + def +(v: Vec2): Vec2 = + Vec2(x + v.x, y + v.y) + + /** Adds this vector to another vector into the target output vector. */ + @inline + def add(v: Vec2, out: Vec2 = this): Vec2 = + out.set(x + v.x, y + v.y) + + /** Adds this vector to another vector into the target output vector. */ + @inline + def add(x: Float, y: Float): Vec2 = + add(x, y, this) + + /** Adds this vector to another vector into the target output vector. */ + @inline + def add(x: Float, y: Float, out: Vec2): Vec2 = + out.set(this.x + x, this.y + y) + + /** Subtracts two vectors. */ + @inline + def -(v: Vec2): Vec2 = + Vec2(x - v.x, y - v.y) + + /** Subtracts a vector from this vector into the given output vector. */ + @inline + def sub(v: Vec2, out: Vec2 = this): Vec2 = + out.set(x - v.x, y - v.y) + + /** Subtracts a vector from this vector into the given output vector. */ + @inline + def sub(x: Float, y: Float): Vec2 = + sub(x, y, this) + + /** Subtracts a vector from this vector into the given output vector. */ + @inline + def sub(x: Float, y: Float, out: Vec2): Vec2 = + out.set(this.x - x, this.y - y) + + /** The dot product of two vectors. */ + @inline + def *(v: Vec2): Float = + x*v.x + y*v.y + + /** Returns the vector scaled by the given scalar. */ + @inline + def *(s: Float): Vec2 = + Vec2(x*s, y*s) + + /** Scales this vector by the given scalar, into the target output vector. */ + @inline + def scale(s: Float, out: Vec2 = this): Vec2 = + out.set(x*s, y*s) + + /** Returns the vector dividied by the given scalar. */ + @inline + def /(s: Float): Vec2 = { + val f = 1/s + Vec2(x*f, y*f) + } + + /** Divides this vector by the given scalar into the target output vector. */ + @inline + def div(s: Float, out: Vec2 = this): Vec2 = + scale(1/s, out) + + /** Negates this vector. */ + @inline + def unary_- = + Vec2(-x, -y) + + /** Negates this vector into the target output vector. */ + @inline + def negate(out: Vec2 = this): Vec2 = + out.set(-x, -y) + + /** Returns the squared magnitude (length2) of this vector. */ + @inline + def magSqr = x*x + y*y + + /** Returns the magnitude (length) of this vector. */ + @inline + def magnitude = math.sqrt(magSqr).toFloat + + /** Returns the normalized vector. */ + @inline + def normalized = this / magnitude + + /** Normalizes this vector into the target output vector. */ + @inline + def normalize(out: Vec2 = this): Vec2 = + out.set(this).div(magnitude) + + @inline + def zNormal(v: Vec2) = x*v.y - y*v.x + + @inline + def max(v: Vec2): Vec2 = + Vec2(v.x max x, v.y max y) + + @inline + def min(v: Vec2): Vec2 = + Vec2(v.x min x, v.y min y) + + @inline + def copy(x: Float = x, y: Float = y): Vec2 = + Vec2(x, y) + + + /** + * Return a vector reflecting this vector about the given normal. + * @note the normal must be normalized. + */ + def reflected(normal: Vec2): Vec2 = + normal * 2*(this*normal) - this + + /** + * Returns the angle between this vector and another, such that a rotation about this angle will align this vector onto the other. + */ + def angleBetween(v: Vec2): Float = { + def wrapPi(angle: Float): Float = + if(angle < -math.Pi) angle + 2*math.Pi.toFloat + else if(angle > math.Pi) angle - 2*math.Pi.toFloat + else angle + wrapPi(math.atan2(v.y, v.x).toFloat - math.atan2(y, x).toFloat) + } + + /** + * Destructively reflect this vector about the given normal. + * @note the normal must be normalized. + */ + def reflect(normal: Vec2, out: Vec2 = this): Vec2 = { + val scale = 2*(this*normal) + out.set(normal.x*scale-x, normal.y*scale-y) + } + /** + * Returns the linear interpolation of this vector with another, with t ranging from 0..1 + */ + @inline + def lerp(q: Vec2, t: Float): Vec2 = + Vec2(x + t*(q.x-x), + y + t*(q.y-y)) + + /** + * Destructively places the linear interpolation of this vector with another into out, with t ranging from 0..1 + */ + def lerp(q: Vec2, t: Float, out: Vec2): Vec2 = + out.set(x + t*(q.x-x), + y + t*(q.y-y)) + + override def toString = + s"Vec2(${x}f,${y}f)" + + override def equals(o: Any): Boolean = o match { + case v: Vec2 => x == v.x && y == v.y + case _ => false + } + + override def hashCode: Int = + x.hashCode() * 19 + y.hashCode() * 23 +} + +object Vec2 { + def apply() = new Vec2() + def apply(x: Float, y: Float) = new Vec2(x, y) } \ No newline at end of file diff --git a/shared/src/main/scala/scryetek/vecmath/Vec3.scala b/shared/src/main/scala/scryetek/vecmath/Vec3.scala index 3b6dac5..9946073 100644 --- a/shared/src/main/scala/scryetek/vecmath/Vec3.scala +++ b/shared/src/main/scala/scryetek/vecmath/Vec3.scala @@ -1,83 +1,196 @@ -package scryetek.vecmath - -import Math._ -/** - * Created by Matt on 29/06/2014. - */ -case class Vec3(var x: Float, var y: Float, var z: Float) { - def this(v: Vec3) = this(v.x, v.y, v.z) - def this() = this(0, 0, 0) - - def set(x: Float, y: Float, z: Float): Vec3 = { - this.x = x; this.y = y; this.z = z; - this - } - - def set(v: Vec3): Vec3 = { - x = v.x; y = v.y; z = v.z - this - } - - def +(o: Vec3): Vec3 = Vec3(x+o.x, y+o.y, z+o.z) - def -(o: Vec3): Vec3 = Vec3(x-o.x, y-o.y, z-o.z) - def *(o: Vec3): Float = x*o.x + y*o.y + z*o.z - def *(s: Float): Vec3 = Vec3(x * s, y*s, z*s) - def /(s: Float): Vec3 = this * (1/s) - - def *(m: Mat4): Vec3 = { - val x1 = x * m.m00 + y * m.m01 + z + m.m02 + m.m03 - val y1 = x * m.m10 + y * m.m11 + z + m.m12 + m.m13 - val z1 = x * m.m20 + y * m.m21 + z + m.m22 + m.m23 - val w = 1/(x * m.m30 + y * m.m31 + z * m.m23 + m.m33) - Vec3(x1*w, y1*w, z1*w) - } - - def unary_- : Vec3 = Vec3(-x, -y, -z) - - def negate: Vec3 = { - x = -x; y = -y; z = -z; - this - } - - def add(v: Vec3): Vec3 = { - x += v.x; y += v.y; z += v.z - this - } - - def sub(v: Vec3): Vec3 = { - x -= v.x; y -= v.y; z -= v.z - this - } - - def scale(s: Float): Vec3 = { - x *= s; y *= s; z *= s; - this - } - - def divide(s: Float) = scale(1/s) - - def crossInto(out: Vec3, v: Vec3): Vec3 = - out.set(y*v.z - z*v.y, - z*v.x - x*v.z, - x*v.y - y*v.x) - - def cross(v: Vec3): Vec3 = - set(y*v.z - z*v.y, - z*v.x - x*v.z, - x*v.y - y*v.x) - - def magSqr: Float = x*x + y*y + z*z - def magnitude: Float = sqrt(magSqr).toFloat - def normalize: Vec3 = { - val len = 1/magnitude - x *= len; y *= len; z *= len - this - } - - override def toString: String = s"Vec3($x, $y, $z)" -} - -object Vec3 { - def apply(): Vec3 = new Vec3() - def apply(v: Vec3) = new Vec3(v) +package scryetek.vecmath + +@inline +final class Vec3(var x: Float, var y: Float, var z: Float) { + @inline + def this() = this(0, 0, 0) + + @inline + def set(x: Float = this.x, y: Float = this.y, z: Float = this.z) = { + this.x = x + this.y = y + this.z = z + this + } + + @inline + def set(v: Vec3): Vec3 = + set(v.x, v.y, v.z) + + /** Adds two vectors. */ + @inline + def +(v: Vec3): Vec3 = + Vec3(x + v.x, y + v.y, z + v.z) + + /** Adds this vector to another vector into the target output vector. */ + @inline + def add(v: Vec3, out: Vec3 = this): Vec3 = + out.set(x + v.x, y + v.y, z + v.z) + + /** Adds this vector to another vector into the target output vector. */ + @inline + def add(x: Float, y: Float, z: Float): Vec3 = + add(x, y, z, this) + + /** Adds this vector to another vector into the target output vector. */ + @inline + def add(x: Float, y: Float, z: Float, out: Vec3): Vec3 = + out.set(this.x + x, this.y + y, this.z + z) + + /** Subtracts two vectors. */ + @inline + def -(v: Vec3): Vec3 = + Vec3(x - v.x, y - v.y, z - v.z) + + /** Subtracts a vector from this vector into the given output vector. */ + @inline + def sub(v: Vec3, out: Vec3 = this): Vec3 = + out.set(x - v.x, y - v.y, z - v.z) + + /** Subtracts a vector from this vector into the given output vector. */ + @inline + def sub(x: Float, y: Float, z: Float, out: Vec3): Vec3 = + out.set(this.x - x, this.y - y, this.z - z) + + /** Subtracts a vector from this vector into the given output vector. */ + @inline + def sub(x: Float, y: Float, z: Float): Vec3 = + sub(x, y, z, this) + + /** The dot product of two vectors. */ + @inline + def *(v: Vec3): Float = + x*v.x + y*v.y + z*v.z + + /** Returns the vector scaled by the given scalar. */ + @inline + def *(s: Float): Vec3 = + Vec3(x*s, y*s, z*s) + + /** Scales this vector by the given scalar, into the target output vector. */ + def scale(s: Float, out: Vec3 = this): Vec3 = + out.set(x*s, y*s, z*s) + + /** Returns the vector dividied by the given scalar. */ + @inline + def /(s: Float): Vec3 = { + val f = 1/s + Vec3(x*f, y*f, z*f) + } + + @inline + def div(s: Float, out: Vec3 = this): Vec3 = + scale(1/s, out) + + @inline + def unary_- = + Vec3(-x, -y, -z) + + @inline + def negate(out: Vec3 = this) = + out.set(-x, -y, -z) + + /** Returns the squared magnitude (length2) of this vector. */ + @inline + def magSqr = x*x + y*y + z*z + + /** Returns the magnitude (length) of this vector. */ + @inline + def magnitude = math.sqrt(magSqr).toFloat + + /** Returns the normalized vector. */ + @inline + def normalized = this / magnitude + + @inline + def normalize(out: Vec3 = this) = + out.set(this).div(magnitude) + + /** Returns the cross product of two vectors. */ + @inline + def ⨯(v: Vec3): Vec3 = + Vec3(y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x) + + /** Returns the cross product of two vectors. */ + @inline + def crossed(v: Vec3): Vec3 = this ⨯ v + + /** Cross products this vector and another into the target output vector. */ + @inline + def cross(v: Vec3, out: Vec3 = this) = + out.set(y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x) + + @inline + def max(v: Vec3): Vec3 = + Vec3(v.x max x, v.y max y, v.z max z) + + @inline + def min(v: Vec3): Vec3 = + Vec3(v.x min x, v.y min y, v.z min z) + + /** + * Return the quaternion that will align this vector with another. + */ + def angleBetween(b: Vec3): Quat = { + val mag = magnitude*b.magnitude + val qx = y*b.z - z*b.y + val qy = z*b.x - x*b.z + val qz = x*b.y - y*b.x + val qw = mag+(this*b) + + Quat(qx, qy, qz, qw).normalized + } + + /** + * Return a vector reflecting this vector about the given normal. + * @note the normal must be normalized. + */ + def reflected(normal: Vec3): Vec3 = + normal * 2*(this*normal) - this + + /** + * Destructively reflect this vector about the given normal. + */ + def reflect(normal: Vec3, out: Vec3 = this): Vec3 = { + val scale = 2*(this*normal) + out.set(normal.x*scale-x, normal.y*scale-y, normal.z*scale-z) + } + + /** + * Returns the linear interpolation of this vector with another, with t ranging from 0..1 + */ + @inline + def lerp(q: Vec3, t: Float): Vec3 = + Vec3(x + t*(q.x-x), + y + t*(q.y-y), + z + t*(q.z-z)) + + /** + * Destructively places the linear interpolation of this vector with another into out, with t ranging from 0..1 + */ + def lerp(q: Vec3, t: Float, out: Vec3): Vec3 = + out.set(x + t*(q.x-x), + y + t*(q.y-y), + z + t*(q.z-z)) + + @inline + def copy(x: Float = x, y: Float = y, z: Float = z): Vec3 = + Vec3(x, y, z) + + override def toString = + s"Vec3(${x}f,${y}f,${z}f)" + + + override def equals(o: Any): Boolean = o match { + case v: Vec3 => x == v.x && y == v.y && z == v.z + case _ => false + } + + override def hashCode: Int = + x.hashCode()*19 + y.hashCode()*23 + z.hashCode()*29 +} + +object Vec3 { + def apply(x: Float, y: Float, z: Float) = new Vec3(x, y, z) + def apply() = new Vec3() } \ No newline at end of file diff --git a/shared/src/main/scala/scryetek/vecmath/Vec4.scala b/shared/src/main/scala/scryetek/vecmath/Vec4.scala index 4f3bc97..901e2bb 100644 --- a/shared/src/main/scala/scryetek/vecmath/Vec4.scala +++ b/shared/src/main/scala/scryetek/vecmath/Vec4.scala @@ -1,73 +1,149 @@ -package scryetek.vecmath - -import java.lang.Math._ - -/** - * Created by Matt on 29/06/2014. - */ -case class Vec4(var x: Float, var y: Float, var z: Float, var w: Float) { - def this(v: Vec4) = this(v.x, v.y, v.z, v.w) - def this() = this(0, 0, 0, 1) - - def set(x: Float, y: Float, z: Float, w: Float): Vec4 = { - this.x = x; this.y = y; this.z = z; this.w = w - this - } - - def set(v: Vec4): Vec4 = { - x = v.x; y = v.y; z = v.z; w = v.w - this - } - - def +(o: Vec4): Vec4 = Vec4(x+o.x, y+o.y, z+o.z, w+o.w) - def -(o: Vec4): Vec4 = Vec4(x-o.x, y-o.y, z-o.z, w-o.w) - def *(o: Vec4): Float = x*o.x + y*o.y + z*o.z + w*o.w - def *(s: Float): Vec4 = Vec4(x*s, y*s, z*s, w*s) - def /(s: Float): Vec4 = this * (1/s) - - def *(m: Mat4): Vec4 = - Vec4( - x * m.m00 + y * m.m01 + z * m.m02 + w * m.m03, - x * m.m10 + y * m.m11 + z * m.m12 + w * m.m13, - x * m.m20 + y * m.m21 + z * m.m22 + w * m.m23, - x * m.m30 + y * m.m31 + z * m.m32 + w * m.m33) - - def unary_- : Vec4 = new Vec4(-x, -y, -z, -w) - - def negate: Vec4 = { - x = -x; y = -y; z = -z; w = -w - this - } - - def add(v: Vec4): Vec4 = { - x += v.x; y += v.y; z += v.z; w += v.w - this - } - - def sub(v: Vec4): Vec4 = { - x -= v.x; y -= v.y; z -= v.z; w -= v.w - this - } - - def scale(s: Float): Vec4 = { - x *= s; y *= s; z *= s; w *= s - this - } - - def divide(s: Float) = scale(1/s) - - def magSqr: Float = x*x + y*y + z*z + w*w - def magnitude: Float = sqrt(magSqr).toFloat - def normalize: Vec4 = { - val len = 1/magnitude - x *= len; y *= len; z *= len; w *= len - this - } - - override def toString: String = s"Vec4($x, $y, $z, $w)" -} - -object Vec4 { - def apply() = new Vec4() - def apply(v: Vec4): Vec4 = new Vec4(v) +package scryetek.vecmath + +@inline +final class Vec4(var x: Float, var y: Float, var z: Float, var w: Float) { + def this() = this(0, 0, 0, 0) + + @inline + def set(x: Float = this.x, y: Float = this.y, z: Float = this.z, w: Float = this.w) = { + this.x = x + this.y = y + this.z = z + this.w = w + this + } + + @inline + def set(v: Vec4): Vec4 = + set(v.x, v.y, v.z, v.w) + + /** Adds two vectors. */ + @inline + def +(v: Vec4): Vec4 = + Vec4(x + v.x, y + v.y, z + v.z, w + v.w) + + @inline + def add(v: Vec4, out: Vec4 = this): Vec4 = + out.set(x + v.x, y + v.y, z + v.z, w + v.w) + + @inline + def add(x: Float, y: Float, z: Float, w: Float): Vec4 = + add(x, y, z, w, this) + + @inline + def add(x: Float, y: Float, z: Float, w: Float, out: Vec4): Vec4 = + out.set(this.x + x, this.y + y, this.z + z, this.w + w) + + /** Subtracts two vectors. */ + @inline + def -(v: Vec4): Vec4 = + Vec4(x - v.x, y - v.y, z - v.z, w - v.w) + + @inline + def sub(v: Vec4, out: Vec4 = this): Vec4 = + out.set(x - v.x, y - v.y, z - v.z, w - v.w) + + @inline + def sub(x: Float, y: Float, z: Float, w: Float, out: Vec4): Vec4 = + out.set(this.x - x, this.y - y, this.z - z, this.w - w) + + @inline + def sub(x: Float, y: Float, z: Float, w: Float): Vec4 = + sub(x, y, z, w, this) + + /** The dot product of two vectors. */ + @inline + def *(v: Vec4): Float = + x*v.x + y*v.y + z*v.z + z*v.z + + /** Returns the vector scaled by the given scalar. */ + @inline + def *(s: Float): Vec4 = + Vec4(x*s, y*s, z*s, w*s) + + @inline + def scale(s: Float, out: Vec4 = this): Vec4 = + out.set(x*s, y*s, z*s, w*s) + + /** Returns the vector dividied by the given scalar. */ + @inline + def /(s: Float): Vec4 = { + val f = 1/s + Vec4(x*f, y*f, z*f, w*f) + } + + @inline + def div(s: Float, out: Vec4 = this): Vec4 = + scale(1/s, out) + + @inline + def unary_- = + Vec4(-x, -y, -z, -w) + + @inline + def negate(out: Vec4 = this) = + out.set(-x, -y, -z, -w) + + /** Returns the squared magnitude (length2) of this vector. */ + @inline + def magSqr = x*x + y*y + z*z + w*w + + @inline + /** Returns the magnitude (length) of this vector. */ + def magnitude = math.sqrt(magSqr).toFloat + + /** Returns the normalized vector. */ + @inline + def normalized = this / magnitude + + @inline + def normalize(out: Vec4 = this) = + out.set(this).div(magnitude) + + @inline + def max(v: Vec4): Vec4 = + Vec4(v.x max x, v.y max y, v.z max z, v.w max w) + + @inline + def min(v: Vec4): Vec4 = + Vec4(v.x min x, v.y min y, v.z min z, v.w max w) + + @inline + def copy(x: Float = x, y: Float = y, z: Float = z, w: Float = w) = + new Vec4(x,y,z,w) + + /** + * Returns the linear interpolation of this vector with another, with t ranging from 0..1 + */ + @inline + def lerp(q: Vec4, t: Float): Vec4 = + Vec4(x + t*(q.x-x), + y + t*(q.y-y), + z + t*(q.z-z), + w + t*(q.w-w)) + + /** + * Destructively places the linear interpolation of this vector with another into out, with t ranging from 0..1 + */ + def lerp(q: Vec4, t: Float, out: Vec4): Vec4 = + out.set(x + t*(q.x-x), + y + t*(q.y-y), + z + t*(q.z-z), + w + t*(q.w-w)) + + override def toString = + s"Vec4(${x}f,${y}f,${z}f,${w}f})" + + override def equals(o: Any): Boolean = o match { + case v: Vec4 => x == v.x && y == v.y && z == v.z && w == v.w + case _ => false + } + + override def hashCode: Int = + x.hashCode()*19 + y.hashCode()*23 + z.hashCode()*29 + w.hashCode()*31 +} + +object Vec4 { + def apply(x: Float, y: Float, z: Float, w: Float) = new Vec4(x,y,z,w) + def apply() = new Vec4() } \ No newline at end of file diff --git a/shared/src/test/scala/scryetek/vecmath/MatSpecification.scala b/shared/src/test/scala/scryetek/vecmath/MatSpecification.scala new file mode 100644 index 0000000..fec5739 --- /dev/null +++ b/shared/src/test/scala/scryetek/vecmath/MatSpecification.scala @@ -0,0 +1,403 @@ +package scryetek.vecmath + +import org.scalacheck._ +import org.scalacheck.Prop._ + +object MatSpecification extends Properties("Matrix") { + property("Mat4 from affine transforms are invertable") = forAll { (m: Mat4, v: Vec3) => + val v2 = Vec4(v.x, v.y, v.z, Math.random().toFloat*5) + m * m.inverted * v2 ~= v2 + } + + property("Mat4.rotate.toQuat * v == Quat.fromAngleAxis * v") = forAll { (a: AngleAxis, v: Vec3) => + val v2 = Vec4(v.x, v.y, v.z, Math.random().toFloat*5) + // the actual quaternion will NOT be the same, so we must test the effect of the transformation. + a.toQuat * v2 ~= Mat4.rotate(a.angle, a.axis).toQuat * v2 + } + + property("Mat3.rotate.toQuat * v == Quat.fromAngleAxis * v") = forAll { (a: AngleAxis, v: Vec3) => + val v2 = Vec4(v.x, v.y, v.z, 1) + // the actual quaternion will NOT be the same, so we must test the effect of the transformation. + a.toQuat * v2 ~= Mat3.rotate(a.angle, a.axis).toQuat * v2 + } + + property("Mat3.rotate * v == Quat.fromAngleAxis.toMat3 * v") = forAll { (a: AngleAxis, v: Vec3) => + // the actual quaternion will NOT be the same, so we must test the effect of the transformation. + a.toQuat.toMat3 * v ~= Mat3.rotate(a.angle, a.axis) * v + } + + property("Mat4().adjoint == Mat4()") = propBoolean { + Mat4().adjointed == Mat4() + } + + property("Mat2().adjoint == Mat2()") = propBoolean { + Mat2().adjointed == Mat2() + } + + property("Mat4: A*/det(A) = A^-1*") = forAll { (m1: Mat4, v: Vec3) => + val v2 = Vec4(v.x, v.y, v.z, 1) + // this is not very numerically pleasant. + (m1.adjointed * (1/m1.determinant) * v2).approximatelyEqualTo(m1.inverted*v2,0.005f) + } + + property("Mat3: A*/det(A) = A^-1*") = forAll { (m1: Mat3, v: Vec2) => + val v2 = Vec3(v.x, v.y, 1) + // this is not very numerically pleasant. + (m1.adjointed * (1/m1.determinant) * v2).approximatelyEqualTo(m1.inverted*v2,0.005f) + } + + property("Mat2: A*/det(A) = A^-1*") = forAll { (m1: Mat2, v: Vec2) => + // this is not very numerically pleasant. + (m1.adjointed * (1/m1.determinant) * v).approximatelyEqualTo(m1.inverted*v,0.05f) + } + + property("Every Mat2d performs an equivalent transformation to it's respective Mat3") = forAll { (m: Mat2d, v: Vec3) => + m * v ~= m.toMat3 * v + } + + property("Every Mat2d performs an equivalent transformation to it's respective Mat4") = forAll { (m: Mat2d, v: Vec3) => + val v4 = m.toMat4 * Vec4(v.x, v.y, v.z, 1) + m * v ~= Vec3(v4.x, v4.y, v4.z) + } + + property("Every Mat2d's inverse performs the equivalent transform as it's respective Mat3's inverse") = forAll { (m: Mat2d, v: Vec3) => + m.inverted * v ~= m.toMat3.inverted * v + } + + + property("Every Mat2d's determinant is the same as it's respective Mat3's determinant") = forAll { (m: Mat2d ) => + m.determinant ~= m.toMat3.determinant + } + + + /// Mutable specifications + property("Mat2.preMultiply is the same as M2 * M1") = forAll { (m1: Mat2, m2: Mat2) => + val outMat = Mat2() + m1.preMultiply(m2,outMat) == m2 * m1 + } + + property("Mat2.postMultiply is the same as M1 * M2") = forAll { (m1: Mat2, m2: Mat2) => + val outMat = Mat2() + m1.postMultiply(m2,outMat) == m1 * m2 + } + + property("Mat2.transpose works the same as Mat2.transposed") = forAll { (m: Mat2) => + val outMat = Mat2() + m.transpose(outMat) == m.transposed + } + + property("Mat2.invert works the same as Mat2.inverted") = forAll { (m: Mat2) => + val outMat = Mat2() + m.invert(outMat) == m.inverted + } + + property("Mat2.adjointInto works the same as Mat2.adjoint") = forAll { (m: Mat2) => + val outMat = Mat2() + m.adjoint(outMat) == m.adjointed + } + + property("Mat2.add is the same as M1 + M2") = forAll { (m1: Mat2, m2: Mat2) => + val outMat = Mat2() + m1.add(m2,outMat) == m2 + m1 + } + + property("Mat2.sub is the same as M1 - M2") = forAll { (m1: Mat2, m2: Mat2) => + val outMat = Mat2() + m1.sub(m2,outMat) == m1 - m2 + } + + property("Mat2 M1 + M2 - M1 == M2") = forAll { (m1: Mat2, m2: Mat2, v: Vec2) => + (m1 + m2 - m1) * v approximatelyEqualTo (m2 * v, 0.01f) + } + + + property("Mat2.mul(v) is the same as M * v") = forAll { (m1: Mat2, v: Vec2) => + val outVec = Vec2() + m1.mul(v, outVec) ~= m1 * v + } + + property("Mat2.postMultiply is the same as M1 * M2") = forAll { (m1: Mat2d, m2: Mat2d) => + val outMat = Mat2d() + m1.postMultiply(m2,outMat) == m1 * m2 + } + + property("Mat2.preMultiply is the same as M2 * M1") = forAll { (m1: Mat2d, m2: Mat2d) => + val outMat = Mat2d() + m1.preMultiply(m2,outMat) == m2 * m1 + } + + property("Mat2d.invert works the same as Mat2d.inverted") = forAll { (m1: Mat2d) => + val outMat = Mat2d() + m1.invert(outMat) == m1.inverted + } + + property("Mat2d.add is the same as M1 + M2") = forAll { (m1: Mat2d, m2: Mat2d) => + val outMat = Mat2d() + m1.add(m2,outMat) == m2 + m1 + } + + property("Mat2d.sub is the same as M1 - M2") = forAll { (m1: Mat2d, m2: Mat2d) => + val outMat = Mat2d() + m1.sub(m2,outMat) == m1 - m2 + } + + property("Mat2d M1 + M2 - M1 == M2") = forAll { (m1: Mat2d, m2: Mat2d, v: Vec3) => + (m1 + m2 - m1) * v approximatelyEqualTo (m2 * v, 0.01f) + } + + property("Mat2d.mul(Vec2) is the same as M * Vec2") = forAll { (m1: Mat2d, v: Vec2) => + val outVec = Vec2() + m1.mul(v, outVec) ~= m1 * v + } + + property("Mat2d.mul(Vec3) is the same as M * Vec3") = forAll { (m1: Mat2d, v: Vec3) => + val outVec = Vec3() + m1.mul(v, outVec) ~= m1 * v + } + + // Mat3 + + property("Mat3.transpose works the same as Mat3.transposed") = forAll { (m1: Mat3) => + val outMat = Mat3() + m1.transpose(outMat) == m1.transposed + } + + property("Mat3.invert works the same as Mat3.inverted") = forAll { (m1: Mat3) => + val outMat = Mat3() + m1.invert(outMat) == m1.inverted + } + + property("Mat3.adjointInto works the same as Mat3.adjoint") = forAll { (m1: Mat3) => + val outMat = Mat3() + m1.adjoint(outMat) == m1.adjointed + } + + property("Mat3.add is the same as M1 + M2") = forAll { (m1: Mat3, m2: Mat3) => + val outMat = Mat3() + m1.add(m2,outMat) == m2 + m1 + } + + property("Mat3.sub is the same as M1 - M2") = forAll { (m1: Mat3, m2: Mat3) => + val outMat = Mat3() + m1.sub(m2,outMat) == m1 - m2 + } + + property("Mat3 M1 + M2 - M1 == M2") = forAll { (m1: Mat3, m2: Mat3, v: Vec3) => + (m1 + m2 - m1) * v approximatelyEqualTo (m2 * v, 0.01f) + } + + property("Mat3.mul(v) is the same as M * v") = forAll { (m1: Mat3, v: Vec3) => + val outVec = Vec3() + m1.mul(v, outVec) ~= m1 * v + } + + property("Mat3.postMultiply is the same as M1 * M2") = forAll { (m1: Mat3, m2: Mat3) => + val outMat = Mat3() + m1.postMultiply(m2,outMat) == m1 * m2 + } + + property("Mat3.preMultiply is the same as M2 * M1") = forAll { (m1: Mat3, m2: Mat3) => + val outMat = Mat3() + m1.preMultiply(m2,outMat) == m2 * m1 + } + + property("Mat3.rotate * m == Mat.preMultiply(rotate)") = forAll { (m: Mat3, a: AngleAxis) => + Mat3.rotate(a.angle, a.axis) * m == m.preRotate(a.angle, a.axis) + } + + property("m * Mat3.rotate == Mat.preMultiply(rotate)") = forAll { (m: Mat3, a: AngleAxis) => + m * Mat3.rotate(a.angle, a.axis) == m.postRotate(a.angle, a.axis) + } + + property("Mat3.scale * m is equivalent to Mat.preScale(v)") = forAll { (m: Mat3, v: Vec3) => + Mat3.scale(v) * m == m.preScale(v) + } + + property("m * Mat3.scale is equivalent to Mat.postScaley(v)") = forAll { (m: Mat3, v: Vec3) => + m * Mat3.scale(v) == m.postScale(v) + } + + property("Mat3.rotateX == Mat.rotate(x, 1, 0, 0)") = forAll { (angle: Float, v: Vec3) => + Mat3.rotateX(angle) * v ~= Mat3.rotate(angle, 1, 0, 0) * v + } + + property("Mat3.rotateX * m == Mat.preRotateX") = forAll { (angle: Float, v: Vec3, m: Mat3) => + Mat3.rotateX(angle) * m * v ~= m.preRotateX(angle) * v + } + + property("m * Mat3.rotateX == Mat.postRotateX") = forAll { (angle: Float, v: Vec3, m: Mat3) => + m * Mat3.rotateX(angle) * v ~= m.postRotateX(angle) * v + } + + + property("Mat3.rotateY == Mat.rotate(a, 0, 1, 0)") = forAll { (angle: Float, v: Vec3) => + Mat3.rotateY(angle) * v ~= Mat3.rotate(angle, 0, 1, 0) * v + } + + property("Mat3.rotateY * m == Mat.preRotateY") = forAll { (angle: Float, v: Vec3, m: Mat3) => + Mat3.rotateY(angle) * m * v ~= m.preRotateY(angle) * v + } + + property("m * Mat3.rotateY == Mat.postRotateY") = forAll { (angle: Float, v: Vec3, m: Mat3) => + m * Mat3.rotateY(angle) * v ~= m.postRotateY(angle) * v + } + + property("Mat3.rotateZ == Mat.rotate(a, 0, 0, 1)") = forAll { (angle: Float, v: Vec3) => + Mat3.rotateZ(angle) * v ~= Mat3.rotate(angle, 0, 0, 1) * v + } + + property("Mat3.rotateZ * m == Mat.preRotateZ") = forAll { (angle: Float, v: Vec3, m: Mat3) => + Mat3.rotateZ(angle) * m * v ~= m.preRotateZ(angle) * v + } + + property("m * Mat3.rotateZ == Mat.postRotateZ") = forAll { (angle: Float, v: Vec3, m: Mat3) => + m * Mat3.rotateZ(angle) * v ~= m.postRotateZ(angle) * v + } + + property("m * q == m * q.toMat3") = forAll { (m: Mat3, q: Quat) => + m * q == m * q.toMat3 + } + + property("m * q == Mat3.postRotateQuat(q)") = forAll { (m: Mat3, q: Quat) => + m * q == m.postRotateQuat(q) + } + + property("q * m == Mat3.postRotateQuat(q)") = forAll { (m: Mat3, q: Quat) => + m * q == m.postRotateQuat(q) + } + + property("q.toMat3 == Mat3.rotateQuat") = forAll { (q: Quat) => + q.toMat3 == Mat3.rotateQuat(q) + } + // Mat4 + + property("Mat4.transpose works the same as Mat4.transposed") = forAll { (m1: Mat4) => + val outMat = Mat4() + m1.transpose(outMat) == m1.transposed + } + + property("Mat4.invert works the same as Mat3.inverted") = forAll { (m: Mat4) => + val outMat = Mat4() + m.invert(outMat) == m.inverted + } + + property("Mat4.adjointInto works the same as Mat3.adjoint") = forAll { (m: Mat4) => + val outMat = Mat4() + m.adjointed(outMat) == m.adjointed + } + + property("Mat4.postMultiply is the same as M1 * M2") = forAll { (m1: Mat4, m2: Mat4) => + val outMat = Mat4() + m1.postMultiply(m2,outMat) == m1 * m2 + } + + property("Mat4.preMultiply is the same as M2 * M1") = forAll { (m1: Mat4, m2: Mat4) => + val outMat = Mat4() + m1.preMultiply(m2,outMat) == m2 * m1 + } + + property("Mat4.add is the same as M1 + M2") = forAll { (m1: Mat4, m2: Mat4) => + val outMat = Mat4() + m1.add(m2,outMat) == m1 + m2 + } + + property("Mat4.sub is the same as M1 - M1") = forAll { (m1: Mat4, m2: Mat4) => + val outMat = Mat4() + m1.sub(m2,outMat) == m1 - m2 + } + + property("Mat4.mul(v) is the same as M * v") = forAll { (m1: Mat4, v: Vec4) => + val outVec = Vec4() + m1.mul(v, outVec) ~= m1 * v + } + + property("Mat4.rotate * m == Mat.preMultiply(rotate)") = forAll { (m: Mat4, a: AngleAxis) => + Mat4.rotate(a.angle, a.axis) * m == m.preRotate(a.angle, a.axis) + } + + property("m * Mat4.rotate == Mat.preMultiply(rotate)") = forAll { (m: Mat4, a: AngleAxis) => + m * Mat4.rotate(a.angle, a.axis) == m.postRotate(a.angle, a.axis) + } + + property("Mat4.scale * m is equivalent to Mat.preScale(v)") = forAll { (m: Mat4, v: Vec3) => + Mat4.scale(v) * m == m.preScale(v) + } + + property("m * Mat4.scale is equivalent to Mat.postScale(v)") = forAll { (m: Mat4, v: Vec3) => + m * Mat4.scale(v) == m.postScale(v) + } + + property("Mat4.translate * m is equivalent to Mat.preTranslate(v)") = forAll { (m: Mat4, v: Vec3) => + Mat4.translate(v) * m == m.preTranslate(v) + } + + property("m * Mat4.translate is equivalent to Mat.postTranslate(v)") = forAll { (m: Mat4, v: Vec3) => + m * Mat4.translate(v) == m.postTranslate(v) + } + + property("Mat4.rotateX == Mat.rotate(x, 1, 0, 0)") = forAll { (angle: Float, v: Vec4) => + Mat4.rotateX(angle) * v ~= Mat4.rotate(angle, 1, 0, 0) * v + } + + property("Mat4.rotateX * m == Mat.preRotateX") = forAll { (angle: Float, v: Vec4, m: Mat4) => + Mat4.rotateX(angle) * m * v ~= m.preRotateX(angle) * v + } + + property("m * Mat4.rotateX == Mat.postRotateX") = forAll { (angle: Float, v: Vec4, m: Mat4) => + m * Mat4.rotateX(angle) * v ~= m.postRotateX(angle) * v + } + + + property("Mat4.rotateY == Mat.rotate(a, 0, 1, 0)") = forAll { (angle: Float, v: Vec4) => + Mat4.rotateY(angle) * v ~= Mat4.rotate(angle, 0, 1, 0) * v + } + + property("Mat4.rotateY * m == Mat.preRotateY") = forAll { (angle: Float, v: Vec4, m: Mat4) => + Mat4.rotateY(angle) * m * v ~= m.preRotateY(angle) * v + } + + property("m * Mat4.rotateY == Mat.postRotateY") = forAll { (angle: Float, v: Vec4, m: Mat4) => + m * Mat4.rotateY(angle) * v ~= m.postRotateY(angle) * v + } + + property("Mat4.rotateZ == Mat.rotate(a, 0, 0, 1)") = forAll { (angle: Float, v: Vec4) => + Mat4.rotateZ(angle) * v ~= Mat4.rotate(angle, 0, 0, 1) * v + } + + property("Mat4.rotateZ * m == Mat.preRotateZ") = forAll { (angle: Float, v: Vec4, m: Mat4) => + Mat4.rotateZ(angle) * m * v ~= m.preRotateZ(angle) * v + } + + property("m * Mat4.rotateZ == Mat.postRotateZ") = forAll { (angle: Float, v: Vec4, m: Mat4) => + m * Mat4.rotateZ(angle) * v ~= m.postRotateZ(angle) * v + } + + property("m * q == m * q.toMat4") = forAll { (m: Mat4, q: Quat) => + m * q == m * q.toMat4 + } + + property("m * q == Mat4.postRotate(q)") = forAll { (m: Mat4, q: Quat) => + m * q == m.postRotateQuat(q) + } + + property("q * m == Mat4.preRotate(q)") = forAll { (m: Mat4, q: Quat) => + m * q == m.postRotateQuat(q) + } + + property("q.toMat4 == Mat4.rotateQuat") = forAll { (q: Quat) => + q.toMat4 == Mat4.rotateQuat(q) + } + + property("Mat4.toAngleAxis.toMat4 == ") = forAll { (a: AngleAxis, v: Vec4) => + val a2 = Mat4.rotate(a.angle, a.axis).toAngleAxis + val v0 = Mat4.rotate(a.angle, a.axis) * v + val v1 = Mat4.rotate(a2.angle, a2.axis) * v + v0 approximatelyEqualTo (v1, 1f) + } + + + property("Mat4 M1 + M2 - M1 == M2") = forAll { (m1: Mat4, m2: Mat4, v: Vec4) => + (m1 + m2 - m1) * v approximatelyEqualTo (m2 * v, 0.01f) + } +} + diff --git a/shared/src/test/scala/scryetek/vecmath/QuatSpecification.scala b/shared/src/test/scala/scryetek/vecmath/QuatSpecification.scala new file mode 100644 index 0000000..7a554cf --- /dev/null +++ b/shared/src/test/scala/scryetek/vecmath/QuatSpecification.scala @@ -0,0 +1,68 @@ +package scryetek.vecmath + +import org.scalacheck._ +import org.scalacheck.Prop._ + +object QuatSpecification extends Properties("Quat") { + property("Quat angle axis to matrix is equivalent to standard rotation matrix") = forAll { (m: AngleAxis, v: Vec4) => + (Mat4.rotate(m.angle, m.axis) * v) ~= (Quat.fromAngleAxis(m.angle, m.axis).toMat4 * v) + } + + property("Quat * Vec4 is equivalent to rotation matrix * Vec4") = forAll { (m: AngleAxis, v: Vec4) => + (Mat4.rotate(m.angle, m.axis) * v) ~= (Quat.fromAngleAxis(m.angle, m.axis) * v) + } + + property("Q1 * Q2 * v == M1 * M2 * v for equivalent rotation matrices M") = forAll { (q: Quat, m: AngleAxis, m2: AngleAxis, v: Vec4) => + (Mat4.rotate(m.angle, m.axis) * Mat4.rotate(m2.angle, m2.axis) * v) ~= (Quat.fromAngleAxis(m.angle, m.axis) * Quat.fromAngleAxis(m2.angle, m2.axis) * v) + } + + property("Quat * Quat.conjugate * v == v") = forAll { (q: Quat, v: Vec4) => + q * q.conjugated * v ~= v + } + + property("q * vec3 is the same as a q * vec4 with w = 1") = forAll { (q: Quat, v: Vec3) => + val v4 = q * Vec4(v.x, v.y, v.z, 1) + q * v approximatelyEqualTo (Vec3(v4.x, v4.y, v4.z), 0.005f) + } + + // Mutable versions + property("q.mul(v) is equivalent to q*v for Vec4") = forAll { (q: Quat, v: Vec4) => + val vOut = Vec4() + val vOld = v.copy() + (q * v ~= q.mul(v, vOut)) && (vOld ~= v) + } + + property("q.mul(v) is equivalent to q*v for Vec3") = forAll { (q: Quat, v: Vec3 ) => + val vOut = Vec3() + val vOld = v.copy() + (q * v ~= q.mul(v, vOut)) && (vOld ~= v) + } + + property("q1.postMultiply(q2) is equivalent to q1 * q1") = forAll { (q1: Quat, q2: Quat) => + val qOut = Quat() + val qOld = q1.copy() + (q1 * q2 == q1.postMultiply(q2, qOut)) && (qOld == q1) + } + + property("Quat.add(Quat) is equivalent to Quat + Quat") = forAll { (q1: Quat, q2: Quat) => + val qOut = Quat() + val qOld = q1.copy() + (q1 + q2 == q1.add(q2, qOut)) && (qOld == q1) + } + + property("Quat.sub(Quat) is equivalent to Quat - Quat") = forAll { (q1: Quat, q2: Quat) => + val qOut = Quat() + val qOld = q1.copy() + (q1 - q2 == q1.sub(q2, qOut)) && (qOld == q1) + } + + property("q1.slerp(q2,1) * v == q2 * v") = forAll { (q1: Quat, q2: Quat, v: Vec3) => + (q1.slerp(q2, 0) * v) approximatelyEqualTo (q1 * v, 0.01f) + } + + property("q1.slerp(q2,0) * v == q1 * v") = forAll { (q1: Quat, q2: Quat, v: Vec3) => + (q1.slerp(q2, 0) * v) approximatelyEqualTo (q1 * v, 0.01f) + } + + +} diff --git a/shared/src/test/scala/scryetek/vecmath/VecSpecification.scala b/shared/src/test/scala/scryetek/vecmath/VecSpecification.scala new file mode 100644 index 0000000..fcafa3f --- /dev/null +++ b/shared/src/test/scala/scryetek/vecmath/VecSpecification.scala @@ -0,0 +1,286 @@ +package scryetek.vecmath + +import org.scalacheck._ +import org.scalacheck.Prop._ + +object VecSpecification extends Properties("Vec") { + property("--Vec2 is identity") = forAll { (a: Vec2) => + -(-a) == a + } + + property("--Vec3 is identity") = forAll { (a: Vec3) => + -(-a) == a + } + + property("--Vec4 is identity") = forAll { (a: Vec4) => + -(-a) == a + } + + property("Vec2 + Vec2 is commutative") = forAll { (a: Vec2, b: Vec2) => + a + b == b + a + } + + property("Vec2 x-x = Vec2(0,0)") = forAll { (a: Vec2) => + a - a ~= Vec2() + } + + + property("Vec3 + Vec3 is commutative") = forAll { (a: Vec3, b: Vec3) => + a + b == b + a + } + + property("Vec3 x-x = Vec3(0,0,0)") = forAll { (a: Vec3) => + a - a ~= Vec3() + } + + property("Vec4 + Vec4 is commutative") = forAll { (a: Vec4, b: Vec4) => + a + b == b + a + } + + property("Vec4 x-x = Vec4(0,0,0,0)") = forAll { (a: Vec4) => + a - a ~= Vec4() + } + + property("Vec2*s/s == Vec2 for s != 0") = forAll(arbitraryVec2.arbitrary, Gen.choose(-100f, 100f) suchThat (math.abs(_) > 0.05f)) { (a: Vec2, s: Float) => + a * s / s ~= a + } + + property("Vec3*s/s == Vec3 for s != 0") = forAll(arbitraryVec3.arbitrary, Gen.choose(-100f, 100f) suchThat (math.abs(_) > 0.05f)) { (a: Vec3, s: Float) => + a * s / s ~= a + } + + property("Vec4*s/s == Vec4 for s != 0") = forAll(arbitraryVec4.arbitrary, Gen.choose(-100f, 100f) suchThat (math.abs(_) > 0.05f)) { (a: Vec4, s: Float) => + a * s / s ~= a + } + + property("Vec2 * Vec2 is commutative") = forAll { (a: Vec2, b: Vec2) => + val z = a * b + (a * b) ~= (b * a) + } + + property("Vec3 * Vec3 is commutative") = forAll { (a: Vec3, b: Vec3) => + val z = a * b + (a * b) ~= (b * a) + } + + property("Vec4 * Vec4 is commutative") = forAll { (a: Vec4, b: Vec4) => + val z = a * b + (a * b) ~= (b * a) + } + + property("Vec2.zNormal works like cross product") = forAll { (a: Vec2, b: Vec2) => + a.zNormal(b) ~= (Vec3(a.x, a.y, 0) crossed Vec3(b.x, b.y, 0)).z + } + + //// Mutable variants. + // we take care to ensure we didn't trash the original value for each of these. + + property("Vec2.add(v) works like +") = forAll { (a: Vec2, b: Vec2) => + val aOld = a.copy() + val bOld = b.copy() + val aOut = Vec2() + (a.add(b, aOut) ~= a + b) && (bOld ~= b) && (aOld ~= a) + } + + property("Vec2.add(v.x,v.y) works like +") = forAll { (a: Vec2, b: Vec2) => + val aOld = a.copy() + val bOld = b.copy() + val aOut = Vec2() + (a.add(b.x, b.y, aOut) ~= a + b) && (bOld ~= b) && (aOld ~= a) + } + + property("Vec2.sub(v) works like -") = forAll { (a: Vec2, b: Vec2) => + val aOld = a.copy() + val bOld = b.copy() + val aOut = Vec2() + (a.sub(b, aOut) ~= a - b) && (bOld ~= b) && (aOld ~= a) + } + + property("Vec2.sub(v.x,v.y) works like -") = forAll { (a: Vec2, b: Vec2) => + val aOld = a.copy() + val bOld = b.copy() + val aOut = Vec2() + (a.sub(b.x, b.y, aOut) ~= a - b) && (bOld ~= b) && (aOld ~= a) + } + + property("Vec2.scale(s) works like v*s") = forAll(arbitraryVec2.arbitrary, Gen.choose(-10f, 10f) suchThat (math.abs(_) > 0.05)) { (a: Vec2, s: Float) => + val aOld = a.copy() + val aOut = Vec2() + (a.scale(s, aOut) ~= a * s) && (aOld ~= a) + } + + property("Vec2.div(s) works like v/s") = forAll(arbitraryVec2.arbitrary, Gen.choose(-10f, 10f) suchThat (math.abs(_) > 0.05)) { (a: Vec2, s: Float) => + val aOld = a.copy() + val aOut = Vec2() + (a.div(s, aOut) ~= a / s) && (aOld ~= a) + } + + property("Vec2.negate() works like -v") = forAll { (a: Vec2) => + val aOld = a.copy() + val aOut = Vec2() + (a.negate(aOut) ~= -a) && (aOld ~= a) + } + + property("Vec2.normalize() works like v.normalized") = forAll { (a: Vec2) => + val aOld = a.copy() + val aOut = Vec2() + (a.normalize(aOut) ~= a.normalized) && (aOld ~= a) + } + + // Vec3 + + property("Vec3.add(v) works like +") = forAll { (a: Vec3, b: Vec3) => + val aOld = a.copy() + val bOld = b.copy() + val aOut = Vec3() + (a.add(b, aOut) ~= a + b) && (bOld ~= b) && (aOld ~= a) + } + + property("Vec3.add(v.x,v.y,v.z) works like +") = forAll { (a: Vec3, b: Vec3) => + val aOld = a.copy() + val bOld = b.copy() + val aOut = Vec3() + (a.add(b.x, b.y, b.z, aOut) ~= a + b) && (bOld ~= b) && (aOld ~= a) + } + + property("Vec3.sub(v) works like -") = forAll { (a: Vec3, b: Vec3) => + val aOld = a.copy() + val bOld = b.copy() + val aOut = Vec3() + (a.sub(b, aOut) ~= a - b) && (bOld ~= b) && (aOld ~= a) + } + + property("Vec3.sub(v.x,v.y) works like -") = forAll { (a: Vec3, b: Vec3) => + val aOld = a.copy() + val bOld = b.copy() + val aOut = Vec3() + (a.sub(b.x, b.y, b.z, aOut) ~= a - b) && (bOld ~= b) && (aOld ~= a) + } + + property("Vec3.scale(s) works like v*s") = forAll(arbitraryVec3.arbitrary, Gen.choose(-10f, 10f) suchThat (math.abs(_) > 0.05)) { (a: Vec3, s: Float) => + val aOld = a.copy() + val aOut = Vec3() + (a.scale(s, aOut) ~= a * s) && (aOld ~= a) + } + + property("Vec3.div(s) works like v/s") = forAll(arbitraryVec3.arbitrary, Gen.choose(-10f, 10f) suchThat (math.abs(_) > 0.05)) { (a: Vec3, s: Float) => + val aOld = a.copy() + val aOut = Vec3() + (a.div(s, aOut) ~= a / s) && (aOld ~= a) + } + + property("Vec3.negate() works like -v") = forAll { (a: Vec3) => + val aOld = a.copy() + val aOut = Vec3() + (a.negate(aOut) ~= -a) && (aOld ~= a) + } + + property("Vec3.normalize() works like v.normalized") = forAll { (a: Vec3) => + val aOld = a.copy() + val aOut = Vec3() + (a.normalize(aOut) ~= a.normalized) && (aOld ~= a) + } + + property("Vec3.cross works like Vec3.crossed") = forAll { (a: Vec3, b: Vec3) => + val aOld = a.copy() + val bOld = b.copy() + val aOut = Vec3() + (a.cross(b, aOut) ~= a crossed b) && (bOld ~= b) && (aOld ~= a) + } + + // Vec4 + + property("Vec4.add(v) works like +") = forAll { (a: Vec4, b: Vec4) => + val aOld = a.copy() + val bOld = b.copy() + val aOut = Vec4() + (a.add(b, aOut) ~= a + b) && (bOld ~= b) && (aOld ~= a) + } + + property("Vec4.add(v.x,v.y,v.z,v.w) works like +") = forAll { (a: Vec4, b: Vec4) => + val aOld = a.copy() + val bOld = b.copy() + val aOut = Vec4() + (a.add(b.x, b.y, b.z, b.w, aOut) ~= a + b) && (bOld ~= b) && (aOld ~= a) + } + + property("Vec4.sub(v) works like -") = forAll { (a: Vec4, b: Vec4) => + val aOld = a.copy() + val bOld = b.copy() + val aOut = Vec4() + (a.sub(b, aOut) ~= a - b) && (bOld ~= b) && (aOld ~= a) + } + + property("Vec4.sub(v.x,v.y,v.z,v.w) works like -") = forAll { (a: Vec4, b: Vec4) => + val aOld = a.copy() + val bOld = b.copy() + val aOut = Vec4() + (a.sub(b.x, b.y, b.z, b.w, aOut) ~= a - b) && (bOld ~= b) && (aOld ~= a) + } + + property("Vec4.scale(s) works like v*s") = forAll(arbitraryVec4.arbitrary, Gen.choose(-10f, 10f) suchThat (math.abs(_) > 0.05)) { (a: Vec4, s: Float) => + val aOld = a.copy() + val aOut = Vec4() + (a.scale(s, aOut) ~= a * s) && (aOld ~= a) + } + + property("Vec4.div(s) works like v/s") = forAll(arbitraryVec4.arbitrary, Gen.choose(-10f, 10f) suchThat (math.abs(_) > 0.05)) { (a: Vec4, s: Float) => + val aOld = a.copy() + val aOut = Vec4() + (a.div(s, aOut) ~= a / s) && (aOld ~= a) + } + + property("Vec4.negate() works like -v") = forAll { (a: Vec4) => + val aOld = a.copy() + val aOut = Vec4() + (a.negate(aOut) ~= -a) && (aOld ~= a) + } + + property("Vec4.normalize() works like v.normalized") = forAll { (a: Vec4) => + val aOld = a.copy() + val aOut = Vec4() + (a.normalize(aOut) ~= a.normalized) && (aOld ~= a) + } + + property("angleBetween two 3D vectors of magnitude M will rotate A onto B") = forAll { (a: Vec3, b: Vec3) => + val q = (a angleBetween b).normalized + a.normalize() + b.normalize() + q * a * 100 approximatelyEqualTo(b * 100, 0.01f) + } + + property("Vec3.reflected is the same as Vec3.reflect") = forAll { (a: Vec3, b: Vec3) => + val aOut = Vec3() + a.reflect(b.normalized, aOut) ~= a.reflected(b.normalized) + } + + property("Vec3.reflected is midway between the two vectors") = forAll { (a: Vec3, n: Vec3) => + a.normalize().reflect(n.normalize()).scale(10) + n.normalize() + + (Math.abs((a angleBetween n).toAngleAxis.angle) < Math.PI/2) ==> { + (a angleBetween n) * (a angleBetween n) * a ~= a.reflected(n) + } + } + + property("angleBetween two 2D vectors of magnitude M will rotate A onto B") = forAll { (a: Vec2, b: Vec2) => + val angle = a angleBetween b + a.normalize() + b.normalize() + Mat2.rotate(angle) * a * 100 ~= b * 100 + } + + property("Vec2.reflected is the same as Vec2.reflect") = forAll { (a: Vec2, b: Vec2) => + val aOut = Vec2() + a.reflect(b.normalized, aOut) ~= a.reflected(b.normalized) + } + + property("Vec2.reflected is midway between the two vectors") = forAll { (a: Vec2, n: Vec2) => + a.normalize() + n.normalize() + + (math.abs(a angleBetween n) < Math.PI/2) ==> { + (a angleBetween n)*2 ~= (a angleBetween a.reflected(n)) + } + } +} diff --git a/shared/src/test/scala/scryetek/vecmath/package.scala b/shared/src/test/scala/scryetek/vecmath/package.scala new file mode 100644 index 0000000..5400d80 --- /dev/null +++ b/shared/src/test/scala/scryetek/vecmath/package.scala @@ -0,0 +1,93 @@ +package scryetek + +import org.scalacheck.Arbitrary._ +import org.scalacheck.Arbitrary +import org.scalacheck.Gen +/** + * Created by Matt on 01/11/2015. + */ +package object vecmath { + implicit val arbitraryVec3 = Arbitrary(for { + x <- Gen.choose(-100f, 100f) + y <- Gen.choose(-100f, 100f) + z <- Gen.choose(-100f, 100f) + vec = Vec3(x,y,z) if !vec.magnitude.isInfinite && !vec.magnitude.isNaN + } yield vec) + + implicit val arbitraryVec2 = Arbitrary(for { + x <- Gen.choose(-100f, 100f) + y <- Gen.choose(-100f, 100f) + vec = Vec2(x,y) if !vec.magnitude.isInfinite && !vec.magnitude.isNaN + } yield vec) + + implicit val arbitraryAngleAxis = Arbitrary(for { + v <- arbitrary[Vec3] + angle <- Gen.choose(-math.Pi*2, math.Pi*2) + } yield AngleAxis(angle.toFloat, v.normalized)) + + implicit val arbitraryVec4 = Arbitrary(for { + x <- Gen.choose(-100f, 100f) + y <- Gen.choose(-100f, 100f) + z <- Gen.choose(-100f, 100f) + w <- Gen.choose(-100f, 100f) + vec = Vec4(x,y,z,w) if !vec.magnitude.isInfinite && !vec.magnitude.isNaN + } yield vec) + + implicit val arbitraryQuat = Arbitrary(for { + axis <- arbitrary[Vec3] + angle <- Gen.choose(-math.Pi*2, math.Pi*2) + } yield Quat.fromAngleAxis(angle.toFloat, axis.normalized)) + + implicit val arbitraryMat4 = Arbitrary(for { + aa <- arbitrary[AngleAxis] + scale <- arbitrary[Vec3] if scale.x != 0 && scale.y != 0 && scale.z != 0 + translate <- arbitrary[Vec3] + } yield Mat4.rotate(aa.angle, aa.axis) * Mat4.translate(translate) * Mat4.scale(scale)) + + implicit val arbitraryMat3 = Arbitrary(for { + aa <- arbitrary[AngleAxis] + scale <- arbitrary[Vec3] if scale.x != 0 && scale.y != 0 && scale.z != 0 + } yield Mat3.rotate(aa.angle, aa.axis) * Mat3.scale(scale)) + + implicit val arbitraryMat2 = Arbitrary(for { + angle <- Gen.choose(-math.Pi*2, math.Pi*2) + scale <- arbitrary[Vec2] if scale.x != 0 && scale.y != 0 + } yield Mat2.rotate(angle.toFloat) * Mat2.scale(scale)) + + implicit val arbitraryMat2d = Arbitrary(for { + m <- arbitrary[Mat3] + } yield m.toMat2d) + + implicit class floatPimp(val f: Float) extends AnyVal { + def approximatelyEqualTo(x: Float, epsilon: Float) = + math.abs(f-x) < epsilon + + def ~=(x: Float) = + this.approximatelyEqualTo(x, 0.001f) + } + + implicit class vec2Pimp(val v: Vec2) { + @inline + def approximatelyEqualTo(v2: Vec2, epsilon: Float): Boolean = + math.abs((v - v2).magnitude) < epsilon + def ~=(v: Vec2): Boolean = + this.approximatelyEqualTo(v, 0.001f) + } + + implicit class vec3Pimp(val v: Vec3) { + @inline + def approximatelyEqualTo(v2: Vec3, epsilon: Float): Boolean = + math.abs((v - v2).magnitude) < epsilon + def ~=(v: Vec3): Boolean = + this.approximatelyEqualTo(v, 0.001f) + } + + implicit class vec4Pimp(val v: Vec4) { + @inline + def approximatelyEqualTo(v2: Vec4, epsilon: Float): Boolean = + math.abs((v - v2).magnitude) < epsilon + def ~=(v: Vec4): Boolean = + this.approximatelyEqualTo(v, 0.001f) + } + +}