diff --git a/raylib/raymath.go b/raylib/raymath.go index 846e6e5..ebd4000 100644 --- a/raylib/raymath.go +++ b/raylib/raymath.go @@ -4,6 +4,55 @@ import ( "math" ) +// Clamp - Clamp float value +func Clamp(value, min, max float32) float32 { + var res float32 + if value < min { + res = min + } else { + res = value + } + + if res > max { + return max + } + + return res +} + +// Lerp - Calculate linear interpolation between two floats +func Lerp(start, end, amount float32) float32 { + var result float32 = start + amount*(end-start) + + return result +} + +// Normalize - Normalize input value within input range +func Normalize(value, start, end float32) float32 { + var result float32 = (value - start) / (end - start) + + return result +} + +// Remap - Remap input value within input range to output range +func Remap(value, inputStart, inputEnd, outputStart, outputEnd float32) float32 { + var result float32 = (value-inputStart)/(inputEnd-inputStart)*(outputEnd-outputStart) + outputStart + + return result +} + +// Wrap - Wrap input value from min to max +func Wrap(value, min, max float32) float32 { + var result float32 = float32(float64(value) - float64(max-min)*math.Floor(float64((value-min)/(max-min)))) + + return result +} + +// FloatEquals - Check whether two given floats are almost equal +func FloatEquals(x, y float32) bool { + return (math.Abs(float64(x-y)) <= 0.000001*math.Max(1.0, math.Max(math.Abs(float64(x)), math.Abs(float64(y))))) +} + // Vector2Zero - Vector with components value 0.0 func Vector2Zero() Vector2 { return NewVector2(0.0, 0.0) @@ -19,16 +68,31 @@ func Vector2Add(v1, v2 Vector2) Vector2 { return NewVector2(v1.X+v2.X, v1.Y+v2.Y) } +// Vector2AddValue - Add vector and float value +func Vector2AddValue(v Vector2, add float32) Vector2 { + return NewVector2(v.X+add, v.Y+add) +} + // Vector2Subtract - Subtract two vectors (v1 - v2) func Vector2Subtract(v1, v2 Vector2) Vector2 { return NewVector2(v1.X-v2.X, v1.Y-v2.Y) } +// Vector2SubtractValue - Subtract vector by float value +func Vector2SubtractValue(v Vector2, sub float32) Vector2 { + return NewVector2(v.X-sub, v.Y-sub) +} + // Vector2Length - Calculate vector length func Vector2Length(v Vector2) float32 { return float32(math.Sqrt(float64((v.X * v.X) + (v.Y * v.Y)))) } +// Vector2LengthSqr - Calculate vector square length +func Vector2LengthSqr(v Vector2) float32 { + return v.X*v.X + v.Y*v.Y +} + // Vector2DotProduct - Calculate two vectors dot product func Vector2DotProduct(v1, v2 Vector2) float32 { return v1.X*v2.X + v1.Y*v2.Y @@ -39,12 +103,23 @@ func Vector2Distance(v1, v2 Vector2) float32 { return float32(math.Sqrt(float64((v1.X-v2.X)*(v1.X-v2.X) + (v1.Y-v2.Y)*(v1.Y-v2.Y)))) } +// Vector2DistanceSqr - Calculate square distance between two vectors +func Vector2DistanceSqr(v1 Vector2, v2 Vector2) float32 { + return (v1.X-v2.X)*(v1.X-v2.X) + (v1.Y-v2.Y)*(v1.Y-v2.Y) +} + // Vector2Angle - Calculate angle from two vectors in radians func Vector2Angle(v1, v2 Vector2) float32 { result := math.Atan2(float64(v2.Y), float64(v2.X)) - math.Atan2(float64(v1.Y), float64(v1.X)) return float32(result) } +// Vector2LineAngle - Calculate angle defined by a two vectors line +// NOTE: Parameters need to be normalized. Current implementation should be aligned with glm::angle +func Vector2LineAngle(start Vector2, end Vector2) float32 { + return float32(-math.Atan2(float64(end.Y-start.Y), float64(end.X-start.X))) +} + // Vector2Scale - Scale vector (multiply by value) func Vector2Scale(v Vector2, scale float32) Vector2 { return NewVector2(v.X*scale, v.Y*scale) @@ -60,8 +135,8 @@ func Vector2Negate(v Vector2) Vector2 { return NewVector2(-v.X, -v.Y) } -// Vector2DivideV - Divide vector by vector -func Vector2DivideV(v1, v2 Vector2) Vector2 { +// Vector2Divide - Divide vector by vector +func Vector2Divide(v1, v2 Vector2) Vector2 { return NewVector2(v1.X/v2.X, v1.Y/v2.Y) } @@ -70,11 +145,113 @@ func Vector2Normalize(v Vector2) Vector2 { return Vector2Scale(v, 1/Vector2Length(v)) } +// Vector2Transform - Transforms a Vector2 by a given Matrix +func Vector2Transform(v Vector2, mat Matrix) Vector2 { + var result = Vector2{} + + var x float32 = v.X + var y float32 = v.Y + var z float32 = 0 + + result.X = mat.M0*x + mat.M4*y + mat.M8*z + mat.M12 + result.Y = mat.M1*x + mat.M5*y + mat.M9*z + mat.M13 + + return result +} + // Vector2Lerp - Calculate linear interpolation between two vectors func Vector2Lerp(v1, v2 Vector2, amount float32) Vector2 { return NewVector2(v1.X+amount*(v2.X-v1.X), v1.Y+amount*(v2.Y-v1.Y)) } +// Vector2Reflect - Calculate reflected vector to normal +func Vector2Reflect(v Vector2, normal Vector2) Vector2 { + var result = Vector2{} + + var dotProduct float32 = v.X*normal.X + v.Y*normal.Y // Dot product + + result.X = v.X - 2.0*normal.X*dotProduct + result.Y = v.Y - 2.0*normal.Y*dotProduct + + return result +} + +// Vector2Rotate - Rotate vector by angle +func Vector2Rotate(v Vector2, angle float32) Vector2 { + var result = Vector2{} + + cosres := float32(math.Cos(float64(angle))) + sinres := float32(math.Sin(float64(angle))) + + result.X = v.X*cosres - v.Y*sinres + result.Y = v.X*sinres + v.Y*cosres + + return result +} + +// Vector2MoveTowards - Move Vector towards target +func Vector2MoveTowards(v Vector2, target Vector2, maxDistance float32) Vector2 { + var result = Vector2{} + + var dx float32 = target.X - v.X + var dy float32 = target.Y - v.Y + var value float32 = dx*dx + dy*dy + + if value == 0 || maxDistance >= 0 && value <= maxDistance*maxDistance { + return target + } + + dist := float32(math.Sqrt(float64(value))) + + result.X = v.X + dx/dist*maxDistance + result.Y = v.Y + dy/dist*maxDistance + + return result +} + +// Vector2Invert - Invert the given vector +func Vector2Invert(v Vector2) Vector2 { + return NewVector2(1.0/v.X, 1.0/v.Y) +} + +// Vector2Clamp - Clamp the components of the vector between min and max values specified by the given vectors +func Vector2Clamp(v Vector2, min Vector2, max Vector2) Vector2 { + var result = Vector2{} + + result.X = float32(math.Min(float64(max.X), math.Max(float64(min.X), float64(v.X)))) + result.Y = float32(math.Min(float64(max.Y), math.Max(float64(min.Y), float64(v.Y)))) + + return result +} + +// Vector2ClampValue - Clamp the magnitude of the vector between two min and max values +func Vector2ClampValue(v Vector2, min float32, max float32) Vector2 { + var result = v + + var length float32 = v.X*v.X + v.Y*v.Y + if length > 0.0 { + length = float32(math.Sqrt(float64(length))) + + if length < min { + var scale float32 = min / length + result.X = v.X * scale + result.Y = v.Y * scale + } else if length > max { + var scale float32 = max / length + result.X = v.X * scale + result.Y = v.Y * scale + } + } + + return result +} + +// Vector2Equals - Check whether two given vectors are almost equal +func Vector2Equals(p Vector2, q Vector2) bool { + return (math.Abs(float64(p.X-q.X)) <= 0.000001*math.Max(1.0, math.Max(math.Abs(float64(p.X)), math.Abs(float64(q.X)))) && + math.Abs(float64(p.Y-q.Y)) <= 0.000001*math.Max(1.0, math.Max(math.Abs(float64(p.Y)), math.Abs(float64(q.Y))))) +} + // Vector2CrossProduct - Calculate two vectors cross product func Vector2CrossProduct(v1, v2 Vector2) float32 { return v1.X*v2.Y - v1.Y*v2.X @@ -90,35 +267,6 @@ func Vector2LenSqr(vector Vector2) float32 { return vector.X*vector.X + vector.Y*vector.Y } -// Mat2Radians - Creates a matrix 2x2 from a given radians value -func Mat2Radians(radians float32) Mat2 { - c := float32(math.Cos(float64(radians))) - s := float32(math.Sin(float64(radians))) - - return NewMat2(c, -s, s, c) -} - -// Mat2Set - Set values from radians to a created matrix 2x2 -func Mat2Set(matrix *Mat2, radians float32) { - cos := float32(math.Cos(float64(radians))) - sin := float32(math.Sin(float64(radians))) - - matrix.M00 = cos - matrix.M01 = -sin - matrix.M10 = sin - matrix.M11 = cos -} - -// Mat2Transpose - Returns the transpose of a given matrix 2x2 -func Mat2Transpose(matrix Mat2) Mat2 { - return NewMat2(matrix.M00, matrix.M10, matrix.M01, matrix.M11) -} - -// Mat2MultiplyVector2 - Multiplies a vector by a matrix 2x2 -func Mat2MultiplyVector2(matrix Mat2, vector Vector2) Vector2 { - return NewVector2(matrix.M00*vector.X+matrix.M01*vector.Y, matrix.M10*vector.X+matrix.M11*vector.Y) -} - // Vector3Zero - Vector with components value 0.0 func Vector3Zero() Vector3 { return NewVector3(0.0, 0.0, 0.0) @@ -134,19 +282,28 @@ func Vector3Add(v1, v2 Vector3) Vector3 { return NewVector3(v1.X+v2.X, v1.Y+v2.Y, v1.Z+v2.Z) } -// Vector3Multiply - Multiply vector by scalar -func Vector3Multiply(v Vector3, scalar float32) Vector3 { - result := Vector3{} - - result.X = v.X * scalar - result.Y = v.Y * scalar - result.Z = v.Z * scalar - - return result +// Vector3AddValue - Add vector and float value +func Vector3AddValue(v Vector3, add float32) Vector3 { + return NewVector3(v.X+add, v.Y+add, v.Z+add) } -// Vector3MultiplyV - Multiply vector by vector -func Vector3MultiplyV(v1, v2 Vector3) Vector3 { +// Vector3Subtract - Subtract two vectors +func Vector3Subtract(v1, v2 Vector3) Vector3 { + return NewVector3(v1.X-v2.X, v1.Y-v2.Y, v1.Z-v2.Z) +} + +// Vector3SubtractValue - Subtract vector by float value +func Vector3SubtractValue(v Vector3, sub float32) Vector3 { + return NewVector3(v.X-sub, v.Y-sub, v.Z-sub) +} + +// Vector3Scale - Scale provided vector +func Vector3Scale(v Vector3, scale float32) Vector3 { + return NewVector3(v.X*scale, v.Y*scale, v.Z*scale) +} + +// Vector3Multiply - Multiply vector by vector +func Vector3Multiply(v1, v2 Vector3) Vector3 { result := Vector3{} result.X = v1.X * v2.X @@ -156,11 +313,6 @@ func Vector3MultiplyV(v1, v2 Vector3) Vector3 { return result } -// Vector3Subtract - Subtract two vectors -func Vector3Subtract(v1, v2 Vector3) Vector3 { - return NewVector3(v1.X-v2.X, v1.Y-v2.Y, v1.Z-v2.Z) -} - // Vector3CrossProduct - Calculate two vectors cross product func Vector3CrossProduct(v1, v2 Vector3) Vector3 { result := Vector3{} @@ -196,6 +348,11 @@ func Vector3Length(v Vector3) float32 { return float32(math.Sqrt(float64(v.X*v.X + v.Y*v.Y + v.Z*v.Z))) } +// Vector3LengthSqr - Calculate vector square length +func Vector3LengthSqr(v Vector3) float32 { + return v.X*v.X + v.Y*v.Y + v.Z*v.Z +} + // Vector3DotProduct - Calculate two vectors dot product func Vector3DotProduct(v1, v2 Vector3) float32 { return v1.X*v2.X + v1.Y*v2.Y + v1.Z*v2.Z @@ -210,6 +367,18 @@ func Vector3Distance(v1, v2 Vector3) float32 { return float32(math.Sqrt(float64(dx*dx + dy*dy + dz*dz))) } +// Vector3DistanceSqr - Calculate square distance between two vectors +func Vector3DistanceSqr(v1 Vector3, v2 Vector3) float32 { + var result float32 = 0.0 + + var dx float32 = v2.X - v1.X + var dy float32 = v2.Y - v1.Y + var dz float32 = v2.Z - v1.Z + result = dx*dx + dy*dy + dz*dz + + return result +} + // Vector3Angle - Calculate angle between two vectors func Vector3Angle(v1 Vector3, v2 Vector3) float32 { var result float32 @@ -222,16 +391,16 @@ func Vector3Angle(v1 Vector3, v2 Vector3) float32 { return result } -// Vector3Scale - Scale provided vector -func Vector3Scale(v Vector3, scale float32) Vector3 { - return NewVector3(v.X*scale, v.Y*scale, v.Z*scale) -} - // Vector3Negate - Negate provided vector (invert direction) func Vector3Negate(v Vector3) Vector3 { return NewVector3(-v.X, -v.Y, -v.Z) } +// Vector3Divide - Divide vector by vector +func Vector3Divide(v1 Vector3, v2 Vector3) Vector3 { + return NewVector3(v1.X/v2.X, v1.Y/v2.Y, v1.Z/v2.Z) +} + // Vector3Normalize - Normalize provided vector func Vector3Normalize(v Vector3) Vector3 { result := v @@ -436,6 +605,211 @@ func Vector3Barycenter(p, a, b, c Vector3) Vector3 { return result } +// Vector3Unproject - Projects a Vector3 from screen space into object space +// NOTE: We are avoiding calling other raymath functions despite available +func Vector3Unproject(source Vector3, projection Matrix, view Matrix) Vector3 { + var result = Vector3{} + + // Calculate unprojected matrix (multiply view matrix by projection matrix) and invert it + var matViewProj = Matrix{ // MatrixMultiply(view, projection); + M0: view.M0*projection.M0 + view.M1*projection.M4 + view.M2*projection.M8 + view.M3*projection.M12, + M4: view.M0*projection.M1 + view.M1*projection.M5 + view.M2*projection.M9 + view.M3*projection.M13, + M8: view.M0*projection.M2 + view.M1*projection.M6 + view.M2*projection.M10 + view.M3*projection.M14, + M12: view.M0*projection.M3 + view.M1*projection.M7 + view.M2*projection.M11 + view.M3*projection.M15, + M1: view.M4*projection.M0 + view.M5*projection.M4 + view.M6*projection.M8 + view.M7*projection.M12, + M5: view.M4*projection.M1 + view.M5*projection.M5 + view.M6*projection.M9 + view.M7*projection.M13, + M9: view.M4*projection.M2 + view.M5*projection.M6 + view.M6*projection.M10 + view.M7*projection.M14, + M13: view.M4*projection.M3 + view.M5*projection.M7 + view.M6*projection.M11 + view.M7*projection.M15, + M2: view.M8*projection.M0 + view.M9*projection.M4 + view.M10*projection.M8 + view.M11*projection.M12, + M6: view.M8*projection.M1 + view.M9*projection.M5 + view.M10*projection.M9 + view.M11*projection.M13, + M10: view.M8*projection.M2 + view.M9*projection.M6 + view.M10*projection.M10 + view.M11*projection.M14, + M14: view.M8*projection.M3 + view.M9*projection.M7 + view.M10*projection.M11 + view.M11*projection.M15, + M3: view.M12*projection.M0 + view.M13*projection.M4 + view.M14*projection.M8 + view.M15*projection.M12, + M7: view.M12*projection.M1 + view.M13*projection.M5 + view.M14*projection.M9 + view.M15*projection.M13, + M11: view.M12*projection.M2 + view.M13*projection.M6 + view.M14*projection.M10 + view.M15*projection.M14, + M15: view.M12*projection.M3 + view.M13*projection.M7 + view.M14*projection.M11 + view.M15*projection.M15} + + // Calculate inverted matrix -> MatrixInvert(matViewProj); + // Cache the matrix values (speed optimization) + var a00 float32 = matViewProj.M0 + var a01 float32 = matViewProj.M1 + var a02 float32 = matViewProj.M2 + var a03 float32 = matViewProj.M3 + var a10 float32 = matViewProj.M4 + var a11 float32 = matViewProj.M5 + var a12 float32 = matViewProj.M6 + var a13 float32 = matViewProj.M7 + var a20 float32 = matViewProj.M8 + var a21 float32 = matViewProj.M9 + var a22 float32 = matViewProj.M10 + var a23 float32 = matViewProj.M11 + var a30 float32 = matViewProj.M12 + var a31 float32 = matViewProj.M13 + var a32 float32 = matViewProj.M14 + var a33 float32 = matViewProj.M15 + + var b00 float32 = a00*a11 - a01*a10 + var b01 float32 = a00*a12 - a02*a10 + var b02 float32 = a00*a13 - a03*a10 + var b03 float32 = a01*a12 - a02*a11 + var b04 float32 = a01*a13 - a03*a11 + var b05 float32 = a02*a13 - a03*a12 + var b06 float32 = a20*a31 - a21*a30 + var b07 float32 = a20*a32 - a22*a30 + var b08 float32 = a20*a33 - a23*a30 + var b09 float32 = a21*a32 - a22*a31 + var b10 float32 = a21*a33 - a23*a31 + var b11 float32 = a22*a33 - a23*a32 + + // Calculate the invert determinant (inlined to avoid double-caching) + var invDet float32 = 1.0 / (b00*b11 - b01*b10 + b02*b09 + b03*b08 - b04*b07 + b05*b06) + + var matViewProjInv = Matrix{ + M0: (a11*b11 - a12*b10 + a13*b09) * invDet, + M4: (-a01*b11 + a02*b10 - a03*b09) * invDet, + M8: (a31*b05 - a32*b04 + a33*b03) * invDet, + M12: (-a21*b05 + a22*b04 - a23*b03) * invDet, + M1: (-a10*b11 + a12*b08 - a13*b07) * invDet, + M5: (a00*b11 - a02*b08 + a03*b07) * invDet, + M9: (-a30*b05 + a32*b02 - a33*b01) * invDet, + M13: (a20*b05 - a22*b02 + a23*b01) * invDet, + M2: (a10*b10 - a11*b08 + a13*b06) * invDet, + M6: (-a00*b10 + a01*b08 - a03*b06) * invDet, + M10: (a30*b04 - a31*b02 + a33*b00) * invDet, + M14: (-a20*b04 + a21*b02 - a23*b00) * invDet, + M3: (-a10*b09 + a11*b07 - a12*b06) * invDet, + M7: (a00*b09 - a01*b07 + a02*b06) * invDet, + M11: (-a30*b03 + a31*b01 - a32*b00) * invDet, + M15: (a20*b03 - a21*b01 + a22*b00) * invDet} + + // Create quaternion from source point + var quat = Quaternion{X: source.X, Y: source.Y, Z: source.Z, W: 1.0} + + // Multiply quat point by unprojecte matrix + var qtransformed = Quaternion{ // QuaternionTransform(quat, matViewProjInv) + X: matViewProjInv.M0*quat.X + matViewProjInv.M4*quat.Y + matViewProjInv.M8*quat.Z + matViewProjInv.M12*quat.W, + Y: matViewProjInv.M1*quat.X + matViewProjInv.M5*quat.Y + matViewProjInv.M9*quat.Z + matViewProjInv.M13*quat.W, + Z: matViewProjInv.M2*quat.X + matViewProjInv.M6*quat.Y + matViewProjInv.M10*quat.Z + matViewProjInv.M14*quat.W, + W: matViewProjInv.M3*quat.X + matViewProjInv.M7*quat.Y + matViewProjInv.M11*quat.Z + matViewProjInv.M15*quat.W} + + // Normalized world points in vectors + result.X = qtransformed.X / qtransformed.W + result.Y = qtransformed.Y / qtransformed.W + result.Z = qtransformed.Z / qtransformed.W + + return result +} + +// Vector3ToFloatV - Get Vector3 as float array +func Vector3ToFloatV(v Vector3) [3]float32 { + var result [3]float32 + + result[0] = v.X + result[1] = v.Y + result[2] = v.Z + + return result +} + +// Vector3Invert - Invert the given vector +func Vector3Invert(v Vector3) Vector3 { + return NewVector3(1.0/v.X, 1.0/v.Y, 1.0/v.Z) +} + +// Vector3Clamp - Clamp the components of the vector between min and max values specified by the given vectors +func Vector3Clamp(v Vector3, min Vector3, max Vector3) Vector3 { + var result = Vector3{} + + result.X = float32(math.Min(float64(max.X), math.Max(float64(min.X), float64(v.X)))) + result.Y = float32(math.Min(float64(max.Y), math.Max(float64(min.Y), float64(v.Y)))) + result.Z = float32(math.Min(float64(max.Z), math.Max(float64(min.Z), float64(v.Z)))) + + return result +} + +// Vector3ClampValue - Clamp the magnitude of the vector between two values +func Vector3ClampValue(v Vector3, min float32, max float32) Vector3 { + var result = v + + var length float32 = v.X*v.X + v.Y*v.Y + v.Z*v.Z + if length > 0.0 { + length = float32(math.Sqrt(float64(length))) + + if length < min { + var scale float32 = min / length + result.X = v.X * scale + result.Y = v.Y * scale + result.Z = v.Z * scale + } else if length > max { + var scale float32 = max / length + result.X = v.X * scale + result.Y = v.Y * scale + result.Z = v.Z * scale + } + } + + return result +} + +// Vector3Equals - Check whether two given vectors are almost equal +func Vector3Equals(p Vector3, q Vector3) bool { + return (math.Abs(float64(p.X-q.X)) <= 0.000001*math.Max(1.0, math.Max(math.Abs(float64(p.X)), math.Abs(float64(q.X)))) && + math.Abs(float64(p.Y-q.Y)) <= 0.000001*math.Max(1.0, math.Max(math.Abs(float64(p.Y)), math.Abs(float64(q.Y)))) && + math.Abs(float64(p.Z-q.Z)) <= 0.000001*math.Max(1.0, math.Max(math.Abs(float64(p.Z)), math.Abs(float64(q.Z))))) +} + +// Vector3Refract - Compute the direction of a refracted ray +// +// v: normalized direction of the incoming ray +// n: normalized normal vector of the interface of two optical media +// r: ratio of the refractive index of the medium from where the ray comes to the refractive index of the medium on the other side of the surface +func Vector3Refract(v Vector3, n Vector3, r float32) Vector3 { + var result = Vector3{} + + var dot float32 = v.X*n.X + v.Y*n.Y + v.Z*n.Z + var d float32 = 1.0 - r*r*(1.0-dot*dot) + + if d >= 0.0 { + d = float32(math.Sqrt(float64(d))) + v.X = r*v.X - (r*dot+d)*n.X + v.Y = r*v.Y - (r*dot+d)*n.Y + v.Z = r*v.Z - (r*dot+d)*n.Z + + result = v + } + + return result +} + +// Mat2Radians - Creates a matrix 2x2 from a given radians value +func Mat2Radians(radians float32) Mat2 { + c := float32(math.Cos(float64(radians))) + s := float32(math.Sin(float64(radians))) + + return NewMat2(c, -s, s, c) +} + +// Mat2Set - Set values from radians to a created matrix 2x2 +func Mat2Set(matrix *Mat2, radians float32) { + cos := float32(math.Cos(float64(radians))) + sin := float32(math.Sin(float64(radians))) + + matrix.M00 = cos + matrix.M01 = -sin + matrix.M10 = sin + matrix.M11 = cos +} + +// Mat2Transpose - Returns the transpose of a given matrix 2x2 +func Mat2Transpose(matrix Mat2) Mat2 { + return NewMat2(matrix.M00, matrix.M10, matrix.M01, matrix.M11) +} + +// Mat2MultiplyVector2 - Multiplies a vector by a matrix 2x2 +func Mat2MultiplyVector2(matrix Mat2, vector Vector2) Vector2 { + return NewVector2(matrix.M00*vector.X+matrix.M01*vector.Y, matrix.M10*vector.X+matrix.M11*vector.Y) +} + // MatrixDeterminant - Compute matrix determinant func MatrixDeterminant(mat Matrix) float32 { var result float32 @@ -553,6 +927,15 @@ func MatrixInvert(mat Matrix) Matrix { return result } +// MatrixIdentity - Returns identity matrix +func MatrixIdentity() Matrix { + return NewMatrix( + 1.0, 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 1.0) +} + // MatrixNormalize - Normalize provided matrix func MatrixNormalize(mat Matrix) Matrix { var result Matrix @@ -579,15 +962,6 @@ func MatrixNormalize(mat Matrix) Matrix { return result } -// MatrixIdentity - Returns identity matrix -func MatrixIdentity() Matrix { - return NewMatrix( - 1.0, 0.0, 0.0, 0.0, - 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 1.0, 0.0, - 0.0, 0.0, 0.0, 1.0) -} - // MatrixAdd - Add two matrices func MatrixAdd(left, right Matrix) Matrix { result := MatrixIdentity() @@ -636,6 +1010,30 @@ func MatrixSubtract(left, right Matrix) Matrix { return result } +// MatrixMultiply - Returns two matrix multiplication +func MatrixMultiply(left, right Matrix) Matrix { + var result Matrix + + result.M0 = left.M0*right.M0 + left.M1*right.M4 + left.M2*right.M8 + left.M3*right.M12 + result.M1 = left.M0*right.M1 + left.M1*right.M5 + left.M2*right.M9 + left.M3*right.M13 + result.M2 = left.M0*right.M2 + left.M1*right.M6 + left.M2*right.M10 + left.M3*right.M14 + result.M3 = left.M0*right.M3 + left.M1*right.M7 + left.M2*right.M11 + left.M3*right.M15 + result.M4 = left.M4*right.M0 + left.M5*right.M4 + left.M6*right.M8 + left.M7*right.M12 + result.M5 = left.M4*right.M1 + left.M5*right.M5 + left.M6*right.M9 + left.M7*right.M13 + result.M6 = left.M4*right.M2 + left.M5*right.M6 + left.M6*right.M10 + left.M7*right.M14 + result.M7 = left.M4*right.M3 + left.M5*right.M7 + left.M6*right.M11 + left.M7*right.M15 + result.M8 = left.M8*right.M0 + left.M9*right.M4 + left.M10*right.M8 + left.M11*right.M12 + result.M9 = left.M8*right.M1 + left.M9*right.M5 + left.M10*right.M9 + left.M11*right.M13 + result.M10 = left.M8*right.M2 + left.M9*right.M6 + left.M10*right.M10 + left.M11*right.M14 + result.M11 = left.M8*right.M3 + left.M9*right.M7 + left.M10*right.M11 + left.M11*right.M15 + result.M12 = left.M12*right.M0 + left.M13*right.M4 + left.M14*right.M8 + left.M15*right.M12 + result.M13 = left.M12*right.M1 + left.M13*right.M5 + left.M14*right.M9 + left.M15*right.M13 + result.M14 = left.M12*right.M2 + left.M13*right.M6 + left.M14*right.M10 + left.M15*right.M14 + result.M15 = left.M12*right.M3 + left.M13*right.M7 + left.M14*right.M11 + left.M15*right.M15 + + return result +} + // MatrixTranslate - Returns translation matrix func MatrixTranslate(x, y, z float32) Matrix { return NewMatrix( @@ -785,6 +1183,41 @@ func MatrixRotateXYZ(ang Vector3) Matrix { return result } +// MatrixRotateZYX - Get zyx-rotation matrix +// NOTE: Angle must be provided in radians +func MatrixRotateZYX(angle Vector3) Matrix { + var result = Matrix{} + + var cz float32 = float32(math.Cos(float64(angle.Z))) + var sz float32 = float32(math.Sin(float64(angle.Z))) + var cy float32 = float32(math.Cos(float64(angle.Y))) + var sy float32 = float32(math.Sin(float64(angle.Y))) + var cx float32 = float32(math.Cos(float64(angle.X))) + var sx float32 = float32(math.Sin(float64(angle.X))) + + result.M0 = cz * cy + result.M4 = cz*sy*sx - cx*sz + result.M8 = sz*sx + cz*cx*sy + result.M12 = float32(0) + + result.M1 = cy * sz + result.M5 = cz*cx + sz*sy*sx + result.M9 = cx*sz*sy - cz*sx + result.M13 = float32(0) + + result.M2 = -sy + result.M6 = cy * sx + result.M10 = cy * cx + result.M14 = float32(0) + + result.M3 = float32(0) + result.M7 = float32(0) + result.M11 = float32(0) + result.M15 = float32(1) + + return result +} + // MatrixScale - Returns scaling matrix func MatrixScale(x, y, z float32) Matrix { result := NewMatrix( @@ -796,30 +1229,6 @@ func MatrixScale(x, y, z float32) Matrix { return result } -// MatrixMultiply - Returns two matrix multiplication -func MatrixMultiply(left, right Matrix) Matrix { - var result Matrix - - result.M0 = left.M0*right.M0 + left.M1*right.M4 + left.M2*right.M8 + left.M3*right.M12 - result.M1 = left.M0*right.M1 + left.M1*right.M5 + left.M2*right.M9 + left.M3*right.M13 - result.M2 = left.M0*right.M2 + left.M1*right.M6 + left.M2*right.M10 + left.M3*right.M14 - result.M3 = left.M0*right.M3 + left.M1*right.M7 + left.M2*right.M11 + left.M3*right.M15 - result.M4 = left.M4*right.M0 + left.M5*right.M4 + left.M6*right.M8 + left.M7*right.M12 - result.M5 = left.M4*right.M1 + left.M5*right.M5 + left.M6*right.M9 + left.M7*right.M13 - result.M6 = left.M4*right.M2 + left.M5*right.M6 + left.M6*right.M10 + left.M7*right.M14 - result.M7 = left.M4*right.M3 + left.M5*right.M7 + left.M6*right.M11 + left.M7*right.M15 - result.M8 = left.M8*right.M0 + left.M9*right.M4 + left.M10*right.M8 + left.M11*right.M12 - result.M9 = left.M8*right.M1 + left.M9*right.M5 + left.M10*right.M9 + left.M11*right.M13 - result.M10 = left.M8*right.M2 + left.M9*right.M6 + left.M10*right.M10 + left.M11*right.M14 - result.M11 = left.M8*right.M3 + left.M9*right.M7 + left.M10*right.M11 + left.M11*right.M15 - result.M12 = left.M12*right.M0 + left.M13*right.M4 + left.M14*right.M8 + left.M15*right.M12 - result.M13 = left.M12*right.M1 + left.M13*right.M5 + left.M14*right.M9 + left.M15*right.M13 - result.M14 = left.M12*right.M2 + left.M13*right.M6 + left.M14*right.M10 + left.M15*right.M14 - result.M15 = left.M12*right.M3 + left.M13*right.M7 + left.M14*right.M11 + left.M15*right.M15 - - return result -} - // MatrixFrustum - Returns perspective projection matrix func MatrixFrustum(left, right, bottom, top, near, far float32) Matrix { var result Matrix @@ -918,6 +1327,65 @@ func MatrixLookAt(eye, target, up Vector3) Matrix { return result } +// MatrixToFloatV - Get float array of matrix data +func MatrixToFloatV(mat Matrix) [16]float32 { + var result [16]float32 + + result[0] = mat.M0 + result[1] = mat.M1 + result[2] = mat.M2 + result[3] = mat.M3 + result[4] = mat.M4 + result[5] = mat.M5 + result[6] = mat.M6 + result[7] = mat.M7 + result[8] = mat.M8 + result[9] = mat.M9 + result[10] = mat.M10 + result[11] = mat.M11 + result[12] = mat.M12 + result[13] = mat.M13 + result[14] = mat.M14 + result[15] = mat.M15 + + return result +} + +// QuaternionAdd - Add two quaternions +func QuaternionAdd(q1 Quaternion, q2 Quaternion) Quaternion { + var result = Quaternion{X: q1.X + q2.X, Y: q1.Y + q2.Y, Z: q1.Z + q2.Z, W: q1.W + q2.W} + + return result +} + +// QuaternionAddValue - Add quaternion and float value +func QuaternionAddValue(q Quaternion, add float32) Quaternion { + var result = Quaternion{X: q.X + add, Y: q.Y + add, Z: q.Z + add, W: q.W + add} + + return result +} + +// QuaternionSubtract - Subtract two quaternions +func QuaternionSubtract(q1 Quaternion, q2 Quaternion) Quaternion { + var result = Quaternion{X: q1.X - q2.X, Y: q1.Y - q2.Y, Z: q1.Z - q2.Z, W: q1.W - q2.W} + + return result +} + +// QuaternionSubtractValue - Subtract quaternion and float value +func QuaternionSubtractValue(q Quaternion, sub float32) Quaternion { + var result = Quaternion{X: q.X - sub, Y: q.Y - sub, Z: q.Z - sub, W: q.W - sub} + + return result +} + +// QuaternionIdentity - Get identity quaternion +func QuaternionIdentity() Quaternion { + var result = Quaternion{W: 1.0} + + return result +} + // QuaternionLength - Compute the length of a quaternion func QuaternionLength(quat Quaternion) float32 { return float32(math.Sqrt(float64(quat.X*quat.X + quat.Y*quat.Y + quat.Z*quat.Z + quat.W*quat.W))) @@ -979,6 +1447,63 @@ func QuaternionMultiply(q1, q2 Quaternion) Quaternion { return result } +// QuaternionScale - Scale quaternion by float value +func QuaternionScale(q Quaternion, mul float32) Quaternion { + var result = Quaternion{} + + result.X = q.X * mul + result.Y = q.Y * mul + result.Z = q.Z * mul + result.W = q.W * mul + + return result +} + +// QuaternionDivide - Divide two quaternions +func QuaternionDivide(q1 Quaternion, q2 Quaternion) Quaternion { + var result = Quaternion{X: q1.X / q2.X, Y: q1.Y / q2.Y, Z: q1.Z / q2.Z, W: q1.W / q2.W} + + return result +} + +// QuaternionLerp - Calculate linear interpolation between two quaternions +func QuaternionLerp(q1 Quaternion, q2 Quaternion, amount float32) Quaternion { + var result = Quaternion{} + + result.X = q1.X + amount*(q2.X-q1.X) + result.Y = q1.Y + amount*(q2.Y-q1.Y) + result.Z = q1.Z + amount*(q2.Z-q1.Z) + result.W = q1.W + amount*(q2.W-q1.W) + + return result +} + +// QuaternionNlerp - Calculate slerp-optimized interpolation between two quaternions +func QuaternionNlerp(q1 Quaternion, q2 Quaternion, amount float32) Quaternion { + var result = Quaternion{} + + // QuaternionLerp(q1, q2, amount) + result.X = q1.X + amount*(q2.X-q1.X) + result.Y = q1.Y + amount*(q2.Y-q1.Y) + result.Z = q1.Z + amount*(q2.Z-q1.Z) + result.W = q1.W + amount*(q2.W-q1.W) + + // QuaternionNormalize(q); + var q = result + var length float32 = float32(math.Sqrt(float64(q.X*q.X + q.Y*q.Y + q.Z*q.Z + q.W*q.W))) + if length == 0.0 { + length = 1.0 + } + var ilength float32 = 1.0 / length + + result.X = q.X * ilength + result.Y = q.Y * ilength + result.Z = q.Z * ilength + result.W = q.W * ilength + + return result +} + // QuaternionSlerp - Calculates spherical linear interpolation between two quaternions func QuaternionSlerp(q1, q2 Quaternion, amount float32) Quaternion { var result Quaternion @@ -1010,6 +1535,35 @@ func QuaternionSlerp(q1, q2 Quaternion, amount float32) Quaternion { return result } +// QuaternionFromVector3ToVector3 - Calculate quaternion based on the rotation from one vector to another +func QuaternionFromVector3ToVector3(from Vector3, to Vector3) Quaternion { + var result = Quaternion{} + + var cos2Theta float32 = from.X*to.X + from.Y*to.Y + from.Z*to.Z // Vector3DotProduct(from, to) + var cross = Vector3{X: from.Y*to.Z - from.Z*to.Y, Y: from.Z*to.X - from.X*to.Z, Z: from.X*to.Y - from.Y*to.X} // Vector3CrossProduct(from, to) + + result.X = cross.X + result.Y = cross.Y + result.Z = cross.Z + result.W = 1.0 + cos2Theta + + // QuaternionNormalize(q); + // NOTE: Normalize to essentially nlerp the original and identity to 0.5 + var q = result + var length float32 = float32(math.Sqrt(float64(q.X*q.X + q.Y*q.Y + q.Z*q.Z + q.W*q.W))) + if length == 0.0 { + length = 1.0 + } + var ilength float32 = 1.0 / length + + result.X = q.X * ilength + result.Y = q.Y * ilength + result.Z = q.Z * ilength + result.W = q.W * ilength + + return result +} + // QuaternionFromMatrix - Returns a quaternion for a given rotation matrix func QuaternionFromMatrix(matrix Matrix) Quaternion { var result Quaternion @@ -1212,18 +1766,14 @@ func QuaternionTransform(q Quaternion, mat Matrix) Quaternion { return result } -// Clamp - Clamp float value -func Clamp(value, min, max float32) float32 { - var res float32 - if value < min { - res = min - } else { - res = value - } - - if res > max { - return max - } - - return res +// QuaternionEquals - Check whether two given quaternions are almost equal +func QuaternionEquals(p, q Quaternion) bool { + return (math.Abs(float64(p.X-q.X)) <= 0.000001*math.Max(1.0, math.Max(math.Abs(float64(p.X)), math.Abs(float64(q.X)))) && + math.Abs(float64(p.Y-q.Y)) <= 0.000001*math.Max(1.0, math.Max(math.Abs(float64(p.Y)), math.Abs(float64(q.Y)))) && + math.Abs(float64(p.Z-q.Z)) <= 0.000001*math.Max(1.0, math.Max(math.Abs(float64(p.Z)), math.Abs(float64(q.Z)))) && + math.Abs(float64(p.W-q.W)) <= 0.000001*math.Max(1.0, math.Max(math.Abs(float64(p.W)), math.Abs(float64(q.W)))) || + math.Abs(float64(p.X+q.X)) <= 0.000001*math.Max(1.0, math.Max(math.Abs(float64(p.X)), math.Abs(float64(q.X)))) && + math.Abs(float64(p.Y+q.Y)) <= 0.000001*math.Max(1.0, math.Max(math.Abs(float64(p.Y)), math.Abs(float64(q.Y)))) && + math.Abs(float64(p.Z+q.Z)) <= 0.000001*math.Max(1.0, math.Max(math.Abs(float64(p.Z)), math.Abs(float64(q.Z)))) && + math.Abs(float64(p.W+q.W)) <= 0.000001*math.Max(1.0, math.Max(math.Abs(float64(p.W)), math.Abs(float64(q.W))))) }