Object appears in unintended parts of the screen when using manual WorldToScreenPoint

Question

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) )
```