commonMain.ru.casperix.math.vector.VectorExtension.kt Maven / Gradle / Ivy

Go to download
package ru.casperix.math.vector

import ru.casperix.math.quaternion.float64.QuaternionDouble
import ru.casperix.math.spherical.float64.SphericalCoordinateDouble
import ru.casperix.math.spherical.float32.SphericalCoordinateFloat
import ru.casperix.math.axis_aligned.float64.Box2d
import ru.casperix.math.axis_aligned.float32.Box2f
import ru.casperix.math.axis_aligned.int32.Box2i
import ru.casperix.math.geometry.Quad2d
import ru.casperix.math.geometry.Quad2f
import ru.casperix.math.geometry.Quad2i
import ru.casperix.math.vector.float32.Vector2f
import ru.casperix.math.vector.float32.Vector3f
import ru.casperix.math.vector.float64.Vector2d
import ru.casperix.math.vector.float64.Vector3d
import ru.casperix.math.vector.int32.Vector2i
import kotlin.math.*

fun getAngle(q1: Vector3d, q2: Vector3d): Double {
    val len = q1.length() * q2.length()
    if (len < 0.000001) return Double.NaN

    val cos = q1.dot(q2) / len
    if (cos.absoluteValue > 0.999999) return 0.0

    return acos(cos)
}

fun getAngle(q1: Vector3f, q2: Vector3f): Float {
    val len = q1.length() * q2.length()
    if (len < 0.000001f) return Float.NaN

    val cos = q1.dot(q2) / len
    if (cos.absoluteValue > 0.999999f) return 0f

    return acos(cos).toFloat()
}

fun getAngle(q1: Vector2d, q2: Vector2d): Double {
    val len = q1.length() * q2.length()
    if (len < 0.000001) return Double.NaN

    val cos = q1.dot(q2) / len
    if (cos.absoluteValue > 0.999999) return 0.0

    return acos(cos)
}

fun maxByLengthOrFirst(A: Vector3d?, B: Vector3d?): Vector3d? {
    if (A == null) {
        return B
    }
    if (B == null) {
        return A
    }
    return if (A.length() > B.length()) A else B
}

fun addOrFirst(A: Vector3d?, B: Vector3d?): Vector3d? {
    if (A == null) {
        return B
    }
    if (B == null) {
        return A
    }
    return A + B
}

fun Vector3d.rotateByAxis(axis: Vector3d, angle: Double): Vector3d {
    val cos = cos(angle)
    val sin = sin(angle)
    val w = (axis.x * x + axis.y * y + axis.z * z) * (1.0 - cos)

    return Vector3d(
        axis.x * w + x * cos + (axis.y * z - axis.z * y) * sin,
        axis.y * w + y * cos + (axis.z * x - axis.x * z) * sin,
        axis.z * w + z * cos + (axis.x * y - axis.y * x) * sin
    )
}

fun Vector2i.rotateCCW(): Vector2i {
    return Vector2i(-y, x)
}

fun Vector2i.rotateCW(): Vector2i {
    return Vector2i(y, -x)
}

fun Vector2d.rotateCCW(): Vector2d {
    return Vector2d(-y, x)
}

fun Vector2f.rotateCCW(): Vector2f {
    return Vector2f(-y, x)
}

fun Vector2f.rotateCW(): Vector2f {
    return Vector2f(y, -x)
}

fun Vector2d.rotateCW(): Vector2d {
    return Vector2d(y, -x)
}

fun Vector2i.rotateBy90(): Vector2i {
    return Vector2i(y, -x)
}

fun Vector2d.rotateBy90(): Vector2d {
    return Vector2d(y, -x)
}


fun Vector3d.asSpherical(): SphericalCoordinateDouble {
    return SphericalCoordinateDouble(x, y, z)
}


fun Vector3d.toSpherical(): SphericalCoordinateDouble {
    val range = sqrt(x * x + y * y + z * z)
    val verticalAngle = acos(z / range)
    val horizontalAngle = atan2(y, x)
    return SphericalCoordinateDouble(range, verticalAngle, horizontalAngle)
}


fun Vector3f.asSpherical(): SphericalCoordinateFloat {
    return SphericalCoordinateFloat(x, y, z)
}


fun Vector3f.toSpherical(): SphericalCoordinateFloat {
    val range = sqrt(x * x + y * y + z * z)
    val verticalAngle = acos(z / range)
    val horizontalAngle = atan2(y, x)
    return SphericalCoordinateFloat(range, verticalAngle, horizontalAngle)
}


fun getRotation(v1: Vector3d, v2: Vector3d): Double {
    return QuaternionDouble.getRotation(v1, v2).norm()
}

fun getProjection(targetDirection: Vector3d, source: Vector3d): Vector3d {
    val direction = targetDirection.normalize()
    val length = source.dot(direction)
    return direction * length
}


fun getProjectionOnPlane(basis1: Vector3d, basis2: Vector3d, source: Vector3d): Vector3d {
    val planeNormal = basis1.cross(basis2).normalize()
    return getProjection(planeNormal, source)
}


fun Box2d.toQuad(): Quad2d {
    return Quad2d(min, Vector2d(max.x, min.y), max, Vector2d(min.x, max.y))
}

fun Box2f.toQuad(): Quad2f {
    return Quad2f(min, Vector2f(max.x, min.y), max, Vector2f(min.x, max.y))
}

fun Box2i.toQuad(): Quad2i {
    return Quad2i(min, Vector2i(max.x, min.y), max, Vector2i(min.x, max.y))
}