commonMain.ru.casperix.math.interpolation.float64.InterpolationDouble.kt Maven / Gradle / Ivy

Go to download
package ru.casperix.math.interpolation.float64

import ru.casperix.math.angle.float64.DegreeDouble
import ru.casperix.math.angle.float64.RadianDouble
import ru.casperix.math.geometry.PI2
import ru.casperix.math.quaternion.float64.QuaternionDouble
import ru.casperix.math.vector.float64.Vector2d
import ru.casperix.math.vector.float64.Vector3d
import ru.casperix.math.vector.float64.Vector4d
import kotlin.math.PI
import kotlin.math.acos
import kotlin.math.sin

object InterpolationDouble {
    fun angle(
        startAngle: DegreeDouble,
        finishAngle: DegreeDouble,
        position: Double,
        interpolator: InterpolateDoubleFunction = linearInterpolate
    ): DegreeDouble {
        return angle(startAngle.toRadian(), finishAngle.toRadian(), position, interpolator).toDegree()
    }


    fun angle(
        startAngle: RadianDouble,
        finishAngle: RadianDouble,
        position: Double,
        interpolator: InterpolateDoubleFunction = linearInterpolate
    ): RadianDouble {
        val start = startAngle.value
        val finish = finishAngle.value
        return RadianDouble(
            if (finish - start <= PI) {
                interpolator(start, finish, position)
            } else if (finish > start) {
                interpolator(start, finish - PI2, position)
            } else {
                interpolator(start - PI2, finish, position)
            }
        )
    }

    fun single(first: Double, second: Double, factor: Double, interpolator: InterpolateDoubleFunction = linearInterpolate): Double {
        return interpolator(first, second, factor)
    }

    fun vector2(first: Vector2d, second: Vector2d, t: Double, interpolator: InterpolateDoubleFunction = linearInterpolate): Vector2d {
        return Vector2d(interpolator(first.x, second.x, t), interpolator(first.y, second.y, t))
    }

    fun vector3(first: Vector3d, second: Vector3d, t: Double, interpolator: InterpolateDoubleFunction = linearInterpolate): Vector3d {
        return Vector3d(
            interpolator(first.x, second.x, t),
            interpolator(first.y, second.y, t),
            interpolator(first.z, second.z, t)
        )
    }

    fun vector4(first: Vector4d, second: Vector4d, t: Double, interpolator: InterpolateDoubleFunction = linearInterpolate): Vector4d {
        return Vector4d(
            interpolator(first.x, second.x, t),
            interpolator(first.y, second.y, t),
            interpolator(first.z, second.z, t),
            interpolator(first.w, second.w, t),
        )
    }

    fun quaternion(q1: QuaternionDouble, q2: QuaternionDouble, factor: Double): QuaternionDouble {
        //	todo: non-linearinterpolation
        val info = setupQD(q1, q2)
        return info.source * sin((1.0 - factor) * info.omega) + info.target * sin(factor * info.omega)
    }

    internal class SlerpD(val source: QuaternionDouble, val target: QuaternionDouble, val omega: Double)

    internal fun setupQD(originalA: QuaternionDouble, originalB: QuaternionDouble): SlerpD {
        val MAX = .999999
        var source = originalA.normalize()
        var target = originalB.normalize()

        var cosOmega = source.dot(target)
        if (cosOmega < 0.0) {
            cosOmega = -cosOmega
            target = -target
        }
        if (cosOmega > MAX) cosOmega = MAX

        val omega = acos(cosOmega)
        val sinOmegaInv = 1.0 / sin(omega)
        source *= sinOmegaInv
        target *= sinOmegaInv
        return SlerpD(source, target, omega)
    }

}