Created
May 30, 2017 16:42
-
-
Save dalibor-drgon/d23746f3d7317f0d2a56c7aef696db53 to your computer and use it in GitHub Desktop.
Inverse kinematics test
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #include <math.h> | |
| typedef struct { | |
| double x, y, z; | |
| } Position3; | |
| typedef struct { | |
| double l1, l2; | |
| double base, j1, j2; | |
| } Angles; | |
| /** | |
| * Using law of Cosine this calculates angle C | |
| * Input length of A B C sides | |
| */ | |
| static inline double cosine_angle(double a, double b, double c) { | |
| return acos((c*c - a*a - b*b) / (-2 * a * b)); | |
| } | |
| /** | |
| * Converts X,Y,Z into angles | |
| * Everything bellow is basic Trigonometry, see Law of Sine, Cosine | |
| */ | |
| __attribute__((noinline)) int translate3(Position3* input, Angles* out) { | |
| //Translate XY coords into base angle | |
| out->base = atan2(input->y, input->x); | |
| //Translate XY coords into single X' cord | |
| //From 3D world to 2D world | |
| double cordX = sqrt(input->y*input->y + input->x+input->x); | |
| //Calculate distance from base to X',Z; and Beta angle | |
| double distance = sqrt(cordX*cordX + input->z+input->z); | |
| out->j2 = cosine_angle(out->l1, out->l2, distance); | |
| //Calculate Alpha angle | |
| double alpha_upper = cosine_angle(out->l1, distance, out->l2); | |
| //double alpha_upper = asin(out->l2 * sin(out->j2) / distance); | |
| double alpha_bottom = atan2(input->z, cordX); | |
| out->j1 = alpha_bottom + alpha_upper; | |
| } | |
| #include <stdio.h> | |
| #include <stdint.h> | |
| #include <Windows.h> | |
| double PCFreq = 0.0; | |
| __int64 CounterStart = 0; | |
| static void StartCounter() | |
| { | |
| LARGE_INTEGER li; | |
| QueryPerformanceFrequency(&li); | |
| PCFreq = (double)(li.QuadPart)/1000.0; | |
| QueryPerformanceCounter(&li); | |
| CounterStart = li.QuadPart; | |
| } | |
| static uint64_t counter() | |
| { | |
| LARGE_INTEGER li; | |
| QueryPerformanceCounter(&li); | |
| __int64 cur = li.QuadPart; | |
| uint64_t ret = (uint64_t) ((double)(cur-CounterStart)/PCFreq); | |
| CounterStart = cur; | |
| return ret; | |
| } | |
| //gcc 3d_to_angles.c -o a.exe -O2 && ./a.exe | |
| int main() { | |
| StartCounter(); | |
| printf("Hi\n"); | |
| int i = 0; | |
| Position3 in = {1.2, 1.2, 3.4}; | |
| Angles out = {10, 15, 0, 0, 0}; | |
| counter(); | |
| for(; i < 1024*1024; i++) { | |
| translate3(&in, &out); | |
| } | |
| unsigned long long v = counter(); | |
| printf("It took: %llu\n", v); | |
| } |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment