0
$\begingroup$

Hy guys I'm new to 3d graphics and i lack some Linear algebra knowledge. I tried to recreate Unity's worldToScreenPoint method with kotlin on android. I copied some code from their forum and wrote missing math with snippets that I found on the internet. I expected to see target put in the middle of the screen, but it appears in different places depending on pitch and yaw of camera.

So here's my code. Could someone please verify validity of my code and math. Here's code to calculate worldToScreenPoint

class TrackingCalc {
    companion object {
        fun worldToLocal(aCamPos: Vector3, aCamRot: Quaternion, aPos: Vector3): Vector3 {
//            Log.i("OnScreen2", (aCamRot.inverse() * (aPos - aCamPos)).toString())
            val result = (aPos - aCamPos)
            val length = result.getMagnitude()
//            val result2 = rotate(aCamRot.inverse(), Quaternion(0.0, result.x, result.y, result.z))
//            Log.i("WorldToLocal", result.toString())
            aCamRot.invTransformPoint(result)
            Log.i("WorldToLocal", result.toString())
            return result
        }

        fun project(aPos: Vector3, aFov: Float, aAspect: Float): Vector3 {
            var f = 1f / tan(Math.toRadians(aFov.toDouble()) / 2)
            f /= aPos.z
            aPos.x *= f / aAspect
            aPos.y *= f
            return aPos
        }

        fun clipSpaceToViewport(aPos: Vector3): Vector3 {
            aPos.x = aPos.x * 0.5f + 0.5f
            aPos.y = aPos.y * 0.5f + 0.5f
            return aPos
        }

        fun worldToViewport(aCamPos: Vector3, aCamRot: Quaternion, aFov: Float, aAspect: Float, aPos: Vector3): Vector3 {
            var p = worldToLocal(aCamPos, aCamRot, aPos)
            p = project(p, aFov, aAspect)
            return clipSpaceToViewport(p)
        }

        fun worldToScreenPos(aCamPos: Vector3, aCamRot: Quaternion, aFov: Float, aScrWidth: Float, aScrHeight: Float, aPos: Vector3): Vector3 {
            var p = worldToViewport(aCamPos, aCamRot, aFov, aScrWidth / aScrHeight, aPos)
            p.x *= aScrWidth
            p.y *= aScrHeight
            return p
        }

        fun worldToGUIPos(aCamPos: Vector3, aCamRot: Quaternion, aFov: Float, aScrWidth: Float, aScrHeight: Float, aPos: Vector3): Vector3 {
            var p = worldToScreenPos(aCamPos, aCamRot, aFov, aScrWidth, aScrHeight, aPos)
            p.y = aScrHeight - p.y
            return p
        }



    }
}

data class Quaternion(var w: Double, var x: Double, var y: Double, var z: Double)

fun Quaternion.inverse(): Quaternion {
    val norm = x*x + y*y + z*z + w*w
    return Quaternion(w / norm, -x / norm, -y / norm, -z / norm)
}
fun Quaternion.rotateYawPitchRoll(rotation: Vector3) {
    rotateYawPitchRoll(rotation.x, rotation.y, rotation.z)
}

fun Quaternion.rotateYawPitchRoll(pitch: Double, yaw: Double, roll: Double) {
    this.rotateY(-yaw)
    this.rotateX(pitch)
    this.rotateZ(roll)
}
fun Quaternion.rotateY(angleDegrees: Double) {
    if (angleDegrees != 0.0) {
        val r = 0.5 * Math.toRadians(angleDegrees)
        rotateY_unsafe(Math.cos(r), Math.sin(r))
    }
}
fun Quaternion.rotateX(angleDegrees: Double) {
    if (angleDegrees != 0.0) {
        val r = 0.5 * Math.toRadians(angleDegrees)
        rotateX_unsafe(Math.cos(r), Math.sin(r))
    }
}

fun Quaternion.rotateZ(angleDegrees: Double) {
    if (angleDegrees != 0.0) {
        val r = 0.5 * Math.toRadians(angleDegrees)
        rotateZ_unsafe(Math.cos(r), Math.sin(r))
    }
}

private fun Quaternion.rotateX_unsafe(fy: Double, fz: Double) {
    val x = x * fy + w * fz
    val y = y * fy + z * fz
    val z = z * fy - this.y * fz
    val w = w * fy - this.x * fz
    this.x = x
    this.y = y
    this.z = z
    this.w = w
    this.normalize()
}

private fun Quaternion.rotateY_unsafe(fx: Double, fz: Double) {
    val x = x * fx - z * fz
    val y = y * fx + w * fz
    val z = z * fx + this.x * fz
    val w = w * fx - this.y * fz
    this.x = x
    this.y = y
    this.z = z
    this.w = w
    this.normalize()
}
private fun Quaternion.rotateZ_unsafe(fx: Double, fy: Double) {
    val x = x * fx + y * fy
    val y = y * fx - this.x * fy
    val z = z * fx + w * fy
    val w = w * fx - this.z * fy
    this.x = x
    this.y = y
    this.z = z
    this.w = w
    this.normalize()
}
private fun Quaternion.normalize() {
    val norm = Math.sqrt((w * w + x * x + y * y + z * z).toDouble()).toFloat()
    this.x /= norm
    this.y /= norm
    this.z /= norm
    this.w /= norm
}
private fun Quaternion.invTransformPoint(point: Vector3) {

    val px: Double = point.x
    val py: Double = point.y
    val pz: Double = point.z
    point.x = ( px + 2.0 * (px*(-y*y-z*z) + py*(x*y+z*w) + pz*(x*z-y*w)) );
    point.y = ( py + 2.0 * (px*(x*y-z*w) + py*(-x*x-z*z) + pz*(y*z+x*w)) );
    point.z = ( pz + 2.0 * (px*(x*z+y*w) + py*(y*z-x*w) + pz*(-x*x-y*y)) );
}

And here's input:

-90 < pitch < 30
0 < heading < 360
fovY = 45f
val quaternion = Quaternion(1.0, 0.0, 0.0, 0.0)
quaternion.rotateYawPitchRoll(-pitch  , -heading, 0.0)
    val onScreen = TrackingCalc.worldToGUIPos(Vector3(camera_x, camera_height, camera_y) , quaternion, 45f, 1920f, 1080f, Vector3(target_x, target_height, target_y) )
```
$\endgroup$
1
  • $\begingroup$ This question is a lot more likely to get answered if you can narrow in on a specific area where you think the problem is. $\endgroup$
    – pmw1234
    Sep 2, 2023 at 10:43

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Browse other questions tagged or ask your own question.