Got a function that uses a lot of trig

private static Vector3 AdjustGravityToNormal(float velocity, Vector3 groundNormal)
{
    if (groundNormal == Vector3.up)
        return Vector3.zero;

    var angle = Mathf.Acos(groundNormal.y);

    var magnitude = velocity * Mathf.Sin(angle);

    var vMagnitude = magnitude * Mathf.Sin(angle);
    var hMagnitude = -magnitude * Mathf.Cos(angle);

    var horizontal = new Vector2(groundNormal.x, groundNormal.z).normalized * hMagnitude;

    return new Vector3(horizontal.x, vMagnitude, horizontal.y);
}

It used to quite regularly return Vector3(NaN, NaN, NaN) when on a flat (Vector3 == Vector3.Up) even though it shouldn't.

Input position is { NaN, NaN, NaN }.

So I added the if (groundNormal == Vector3.up) return Vector3.zero; part and it got rid of most of the errors. But I still get the very rare occasional one and I'd like to get rid of it.

Any ideas?

Bonus Question which will probably solve original question so I'll leave it in same answer. Haven't really done vectors/trig in maybe 6 years so I'm a bit rusty. But does anyone know how to rewrite my function a lot cleaner? I'm pretty sure there are ways to do it using cross products etc. Pretty much the function is manually calculating the force in the x,y,z direction on a slope for gravity.