diff options
Diffstat (limited to 'trochoid_math.cginc')
| -rw-r--r-- | trochoid_math.cginc | 195 |
1 files changed, 132 insertions, 63 deletions
diff --git a/trochoid_math.cginc b/trochoid_math.cginc index 7a4e7e2..dd6d69c 100644 --- a/trochoid_math.cginc +++ b/trochoid_math.cginc @@ -6,83 +6,152 @@ #if defined(_TROCHOID) -float3 trochoid_map(float theta, float r0, float3 vert_z) +#define TROCH_POSITION_SCALE 0.1 +#define TROCH_Z_THETA_SCALE 0.0002 +#define TROCH_EPSILON 1e-5 + +float3 cyl2_to_troch_map(float3 pos) { - r0 *= r0; - r0 *= 100; + float theta = pos.x; float R = _Trochoid_R; float r = _Trochoid_r; float d = _Trochoid_d; - theta *= max(R, r); - float theta_t = theta + _Time[2]; + float x = ((R - r) * cos(theta * R) + d * cos((R - r) * theta * R / r)) * pos.y * TROCH_POSITION_SCALE; + float y = ((R - r) * sin(theta * R) - d * sin((R - r) * theta * R / r)) * pos.y * TROCH_POSITION_SCALE; + float z = (pos.z + cos(theta * R * 5) * TROCH_POSITION_SCALE + theta * R * TROCH_Z_THETA_SCALE) * pos.y; - float x = (R - r) * cos(theta_t) + d * cos((R - r) * theta_t / r); - float y = (R - r) * sin(theta_t) - d * sin((R - r) * theta_t / r); - float z = vert_z + cos(theta_t * 5) * .1 + theta * .0002; + return float3(x, y, z); +} - float3 result = float3(x, y, z) * r0; - result.xy *= 0.1; - return result; +float3 cyl_to_cyl2_map(float3 v) +{ + return float3(v.x, pow(v.y, _Trochoid_Radius_Power) * _Trochoid_Radius_Scale, v.z * _Trochoid_Height_Scale); } -float trochoid_normal(float3 objPos, float2 uv) +float3 cart_to_cyl_map(float3 v) { - float theta = uv.x * TAU; - float r0 = length(objPos.xyz); - - float x = objPos.x; - float y = objPos.y; - float z = objPos.z; - - float e = 5E-2; - float small_step = 1E-2 * e; - float du_dt = (trochoid_map(theta + small_step, r0, z) - trochoid_map(theta - small_step, r0, z)) / small_step; - small_step = 1E-4 * e; - float du_dr = (trochoid_map(theta, r0 + small_step, z) - trochoid_map(theta, r0 - small_step, z)) / small_step; - small_step = 1E-5 * e; - float du_dz = (trochoid_map(theta, r0, z + small_step) - trochoid_map(theta, r0, z - small_step)) / small_step; - - // U(T(x, y, z), R(x, y, z), Z(x, y, z)) - // T(x, y, z) = atan2(y, x) - // R(x, y, z) = length(float3(x, y, z)) - // Z(x, y, z) = z - // U(a, b, c) = trochoid_map(a, b, c) - // dU/dx = dU/dT dT/dx + dU/dR dR/dx + dU/dZ dZ/dx - // dU/dy = dU/dT dT/dy + dU/dR dR/dy + dU/dZ dZ/dy - // dU/dz = dU/dT dT/dz + dU/dR dR/dz + dU/dZ dZ/dz - // dT/dx = d/dx atan2(y, x) = -y / (x**2 + y**2) - // dT/dy = d/dx atan2(y, x) = x / (x**2 + y**2) - // dT/dz = d/dz atan2(y, x) = 0 - // dR/dx = d/dx sqrt(x**2 + y**2 + z**2) = x / sqrt(x**2 + y**2 + z**2) - // dR/dy = d/dy sqrt(x**2 + y**2 + z**2) = y / sqrt(x**2 + y**2 + z**2) - // dR/dz = d/dy sqrt(x**2 + y**2 + z**2) = z / sqrt(x**2 + y**2 + z**2) - // dZ/dx = 0 - // dZ/dy = 0 - // dZ/dz = 1 - float xy_norm = sqrt(x * x + y * y); - float dt_dx = -y / xy_norm; - float dt_dy = x / xy_norm; - float dt_dz = 0; - float xyz_norm = sqrt(x * x + y * y + z * z); - float dr_dx = x / xyz_norm; - float dr_dy = y / xyz_norm; - float dr_dz = z / xyz_norm; - float dz_dx = 0; - float dz_dy = 0; - float dz_dz = 1; - - float3 normal = - normalize( - float3( - du_dt * dt_dx + du_dr * dr_dx + du_dz * dz_dx, - du_dt * dt_dy + du_dr * dr_dy + du_dz * dz_dy, - du_dt * dt_dz + du_dr * dr_dz + du_dz * dz_dz)); - return UnityObjectToWorldNormal(normal); + return float3(atan2(v.y, v.x), length(v.xy), v.z); +} + +// Compute partial derivatives of trochoid function with respect to cylindrical coordinates +float3x3 cyl2_to_troch_jacobian(float3 pos) +{ + float theta = pos.x; + + float R = _Trochoid_R; + float r = _Trochoid_r; + float d = _Trochoid_d; + +#if 1 + float3 df_dt = float3( + ((R * (r - R) * (d * sin(R * theta * (R - r) / r) + r * sin(R * theta))) / r) * pos.y * TROCH_POSITION_SCALE, + ((R * (r - R) * (-d * cos(R * theta * (R - r) / r) + r * cos(R * theta))) / r) * pos.y * TROCH_POSITION_SCALE, + (-R * 5 * sin(theta * R * 5) * TROCH_POSITION_SCALE + R * TROCH_Z_THETA_SCALE) * pos.y); + float3 df_dr = float3( + ((R - r) * cos(theta * R) + d * cos((R - r) * theta * R / r)) * TROCH_POSITION_SCALE, + ((R - r) * sin(theta * R) - d * sin((R - r) * theta * R / r)) * TROCH_POSITION_SCALE, + (pos.z + cos(theta * R * 5) * TROCH_POSITION_SCALE + theta * R * TROCH_Z_THETA_SCALE)); + float3 df_dz = float3( + 0, + 0, + pos.y); +#else + float3 df_dt = (cyl2_to_troch_map(pos + float3(TROCH_EPSILON, 0, 0)) - cyl2_to_troch_map(pos - float3(TROCH_EPSILON, 0, 0))) / (2 * TROCH_EPSILON); + float3 df_dr = (cyl2_to_troch_map(pos + float3(0, TROCH_EPSILON, 0)) - cyl2_to_troch_map(pos - float3(0, TROCH_EPSILON, 0))) / (2 * TROCH_EPSILON); + float3 df_dz = (cyl2_to_troch_map(pos + float3(0, 0, TROCH_EPSILON)) - cyl2_to_troch_map(pos - float3(0, 0, TROCH_EPSILON))) / (2 * TROCH_EPSILON); +#endif + float3x3 jacobian_cyl; + jacobian_cyl[0] = df_dt; + jacobian_cyl[1] = df_dr; + jacobian_cyl[2] = df_dz; + return transpose(jacobian_cyl); +} + +float3x3 cyl_to_cyl2_jacobian(float3 pos) +{ + // f(x, y, z) = <x, (y^_Trochoid_Radius_Power) * _Trochoid_Radius_Scale, z * _Trochoid_Height_Scale> +#if 1 + float3 df_dx = float3(1, 0, 0); + float3 df_dy = float3(0, _Trochoid_Radius_Power * pow(pos.y, _Trochoid_Radius_Power - 1) * _Trochoid_Radius_Scale, 0); + float3 df_dz = float3(0, 0, _Trochoid_Height_Scale); +#else + float3 df_dx = (cyl_to_cyl2_map(pos + float3(TROCH_EPSILON, 0, 0)) - cyl_to_cyl2_map(pos - float3(TROCH_EPSILON, 0, 0))) / (2 * TROCH_EPSILON); + float3 df_dy = (cyl_to_cyl2_map(pos + float3(0, TROCH_EPSILON, 0)) - cyl_to_cyl2_map(pos - float3(0, TROCH_EPSILON, 0))) / (2 * TROCH_EPSILON); + float3 df_dz = (cyl_to_cyl2_map(pos + float3(0, 0, TROCH_EPSILON)) - cyl_to_cyl2_map(pos - float3(0, 0, TROCH_EPSILON))) / (2 * TROCH_EPSILON); +#endif + float3x3 jacobian_cyl_to_cyl2; + jacobian_cyl_to_cyl2[0] = df_dx; + jacobian_cyl_to_cyl2[1] = df_dy; + jacobian_cyl_to_cyl2[2] = df_dz; + return transpose(jacobian_cyl_to_cyl2); +} + +// Compute partial derivatives of transform from cartesian to cylindrical coordinates +float3x3 cart_to_cyl_jacobian(float3 pos) +{ + // Compute partial derivatives of transform from cartesian to cylindrical coordinates + // return float3(atan2(v.y, v.x), length(v.xy), v.z); + // theta = atan2(y, x) +#if 1 + float3 dtheta_dcart = float3( + -pos.y / dot(pos.xy, pos.xy), + pos.x / dot(pos.xy, pos.xy), + 0); +#else + float3 dtheta_dcart = (cart_to_cyl_map(pos + float3(TROCH_EPSILON, 0, 0)) - cart_to_cyl_map(pos - float3(TROCH_EPSILON, 0, 0))) / (2 * TROCH_EPSILON); +#endif + + // radius = (x^2 + y^2)^(1/2) +#if 1 + float3 dr_dcart = float3( + pos.x / sqrt(pos.x * pos.x + pos.y * pos.y), + pos.y / sqrt(pos.x * pos.x + pos.y * pos.y), + 0); +#else + float3 dr_dcart = (cart_to_cyl_map(pos + float3(0, TROCH_EPSILON, 0)) - cart_to_cyl_map(pos - float3(0, TROCH_EPSILON, 0))) / (2 * TROCH_EPSILON); +#endif + +#if 1 + float3 dz_dcart = float3( + 0, + 0, + 1); +#else + float3 dz_dcart = (cart_to_cyl_map(pos + float3(0, 0, TROCH_EPSILON)) - cart_to_cyl_map(pos - float3(0, 0, TROCH_EPSILON))) / (2 * TROCH_EPSILON); +#endif + float3x3 jacobian_cart_to_cyl; + jacobian_cart_to_cyl[0] = dtheta_dcart; + jacobian_cart_to_cyl[1] = dr_dcart; + jacobian_cart_to_cyl[2] = dz_dcart; + return transpose(jacobian_cart_to_cyl); +} + +float3x3 invert(float3x3 m) +{ + float det = m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]) + - m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0]) + + m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]); + + float3x3 adj; + adj[0][0] = (m[1][1] * m[2][2] - m[1][2] * m[2][1]); + adj[0][1] = -(m[0][1] * m[2][2] - m[0][2] * m[2][1]); + adj[0][2] = (m[0][1] * m[1][2] - m[0][2] * m[1][1]); + + adj[1][0] = -(m[1][0] * m[2][2] - m[1][2] * m[2][0]); + adj[1][1] = (m[0][0] * m[2][2] - m[0][2] * m[2][0]); + adj[1][2] = -(m[0][0] * m[1][2] - m[0][2] * m[1][0]); + + adj[2][0] = (m[1][0] * m[2][1] - m[1][1] * m[2][0]); + adj[2][1] = -(m[0][0] * m[2][1] - m[0][1] * m[2][0]); + adj[2][2] = (m[0][0] * m[1][1] - m[0][1] * m[1][0]); + + return adj * (1.0 / det); } #endif // _TROCHOID #endif // __TROCHOID_MATH + |