diff options
Diffstat (limited to 'trochoid_math.cginc')
| -rw-r--r-- | trochoid_math.cginc | 139 |
1 files changed, 74 insertions, 65 deletions
diff --git a/trochoid_math.cginc b/trochoid_math.cginc index dd6d69c..3a5107e 100644 --- a/trochoid_math.cginc +++ b/trochoid_math.cginc @@ -10,24 +10,30 @@ #define TROCH_Z_THETA_SCALE 0.0002 #define TROCH_EPSILON 1e-5 -float3 cyl2_to_troch_map(float3 pos) +float3 cyl2_to_troch_map(float3 v) { - float theta = pos.x; - - float R = _Trochoid_R; - float r = _Trochoid_r; - float d = _Trochoid_d; - - 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; + const float R = _Trochoid_R; + const float r = _Trochoid_r; + const float d = _Trochoid_d; + const float rrrr = (R - r) * R / r; + + float x = + cos(v.x * R) * v.y * (R - r) * TROCH_POSITION_SCALE + + cos(v.x * rrrr) * v.y * d * TROCH_POSITION_SCALE; + float y = + sin(v.x * R) * v.y * (R - r) * TROCH_POSITION_SCALE - + sin(v.x * rrrr) * v.y * d * TROCH_POSITION_SCALE; + float z = + (v.x * v.y * R * TROCH_Z_THETA_SCALE + + cos(v.x * R * 5) * v.y * TROCH_POSITION_SCALE + + v.y * v.z); return float3(x, y, z); } 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); + return float3(v.x * .5, pow(v.y, _Trochoid_Radius_Power) * _Trochoid_Radius_Scale, v.z * _Trochoid_Height_Scale); } float3 cart_to_cyl_map(float3 v) @@ -35,33 +41,60 @@ float3 cart_to_cyl_map(float3 v) 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) +float3 cart_to_troch_map(float3 v) { - float theta = pos.x; + return cyl2_to_troch_map(cyl_to_cyl2_map(cart_to_cyl_map(v))); +} - float R = _Trochoid_R; - float r = _Trochoid_r; - float d = _Trochoid_d; +float3x3 cart_to_troch_jacobian(float3 v) +{ + float epsilon = 1e-5; + float3 df_dx = (cart_to_troch_map(v + float3(epsilon, 0, 0)) - cart_to_troch_map(v - float3(epsilon, 0, 0))) / (2 * epsilon); + float3 df_dy = (cart_to_troch_map(v + float3(0, epsilon, 0)) - cart_to_troch_map(v - float3(0, epsilon, 0))) / (2 * epsilon); + float3 df_dz = (cart_to_troch_map(v + float3(0, 0, epsilon)) - cart_to_troch_map(v - float3(0, 0, epsilon))) / (2 * epsilon); + return transpose(float3x3(df_dx, df_dy, df_dz)); +} + +// Compute partial derivatives of trochoid function with respect to cylindrical coordinates +float3x3 cyl2_to_troch_jacobian(float3 v) +{ + const float R = _Trochoid_R; + const float r = _Trochoid_r; + const float d = _Trochoid_d; + const float rrrr = (R - r) * R / r; #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); + -R * sin(v.x * R) * v.y * (R - r) * TROCH_POSITION_SCALE + + -rrrr * sin(v.x * rrrr) * v.y * d * TROCH_POSITION_SCALE, + + R * cos(v.x * R) * v.y * (R - r) * TROCH_POSITION_SCALE - + rrrr * cos(v.x * rrrr) * v.y * d * TROCH_POSITION_SCALE, + + v.y * R * TROCH_Z_THETA_SCALE - + R * 5 * sin(v.x * R * 5) * v.y * TROCH_POSITION_SCALE); +#else + float3 df_dt = (cyl2_to_troch_map(v + float3(TROCH_EPSILON, 0, 0)) - cyl2_to_troch_map(v - float3(TROCH_EPSILON, 0, 0))) / (2 * TROCH_EPSILON); +#endif + +#if 1 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)); + ((R - r) * cos(v.x * R) + d * cos((R - r) * v.x * R / r)) * TROCH_POSITION_SCALE, + ((R - r) * sin(v.x * R) - d * sin((R - r) * v.x * R / r)) * TROCH_POSITION_SCALE, + cos(v.x * R * 5) * TROCH_POSITION_SCALE); +#else + float3 df_dr = (cyl2_to_troch_map(v + float3(0, TROCH_EPSILON, 0)) - cyl2_to_troch_map(v - float3(0, TROCH_EPSILON, 0))) / (2 * TROCH_EPSILON); +#endif + +#if 1 float3 df_dz = float3( 0, 0, - pos.y); + v.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); + float3 df_dz = (cyl2_to_troch_map(v + float3(0, 0, TROCH_EPSILON)) - cyl2_to_troch_map(v - float3(0, 0, TROCH_EPSILON))) / (2 * TROCH_EPSILON); #endif + float3x3 jacobian_cyl; jacobian_cyl[0] = df_dt; jacobian_cyl[1] = df_dr; @@ -69,17 +102,17 @@ float3x3 cyl2_to_troch_jacobian(float3 pos) return transpose(jacobian_cyl); } -float3x3 cyl_to_cyl2_jacobian(float3 pos) +float3x3 cyl_to_cyl2_jacobian(float3 v) { - // f(x, y, z) = <x, (y^_Trochoid_Radius_Power) * _Trochoid_Radius_Scale, z * _Trochoid_Height_Scale> + // f(x, y, z) = <x, (y^_Trochoid_Radiu1_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_dy = float3(0, _Trochoid_Radius_Power * pow(v.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); + float3 df_dx = (cyl_to_cyl2_map(v + float3(TROCH_EPSILON, 0, 0)) - cyl_to_cyl2_map(v - float3(TROCH_EPSILON, 0, 0))) / (2 * TROCH_EPSILON); + float3 df_dy = (cyl_to_cyl2_map(v + float3(0, TROCH_EPSILON, 0)) - cyl_to_cyl2_map(v - float3(0, TROCH_EPSILON, 0))) / (2 * TROCH_EPSILON); + float3 df_dz = (cyl_to_cyl2_map(v + float3(0, 0, TROCH_EPSILON)) - cyl_to_cyl2_map(v - float3(0, 0, TROCH_EPSILON))) / (2 * TROCH_EPSILON); #endif float3x3 jacobian_cyl_to_cyl2; jacobian_cyl_to_cyl2[0] = df_dx; @@ -89,28 +122,28 @@ float3x3 cyl_to_cyl2_jacobian(float3 pos) } // Compute partial derivatives of transform from cartesian to cylindrical coordinates -float3x3 cart_to_cyl_jacobian(float3 pos) +float3x3 cart_to_cyl_jacobian(float3 v) { // 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), + -v.y / dot(v.xy, v.xy), + v.x / dot(v.xy, v.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); + float3 dtheta_dcart = (cart_to_cyl_map(v + float3(TROCH_EPSILON, 0, 0)) - cart_to_cyl_map(v - 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), + v.x / sqrt(v.x * v.x + v.y * v.y), + v.y / sqrt(v.x * v.x + v.y * v.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); + float3 dr_dcart = (cart_to_cyl_map(v + float3(0, TROCH_EPSILON, 0)) - cart_to_cyl_map(v - float3(0, TROCH_EPSILON, 0))) / (2 * TROCH_EPSILON); #endif #if 1 @@ -119,7 +152,7 @@ float3x3 cart_to_cyl_jacobian(float3 pos) 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); + float3 dz_dcart = (cart_to_cyl_map(v + float3(0, 0, TROCH_EPSILON)) - cart_to_cyl_map(v - float3(0, 0, TROCH_EPSILON))) / (2 * TROCH_EPSILON); #endif float3x3 jacobian_cart_to_cyl; jacobian_cart_to_cyl[0] = dtheta_dcart; @@ -128,30 +161,6 @@ float3x3 cart_to_cyl_jacobian(float3 pos) 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 - - |