fork download 
  1. #include <math.h>
  2. #include <assert.h>
  3. #include <stdio.h>
  4.  
  5. struct vector3_t
  6. {
  7. 	float x, y, z;
  8. };
  9.  
  10. struct matrix3_t
  11. {
  12. 	union
  13. 	{
  14. 		struct
  15. 		{
  16. 			float	_11, _12, _13;
  17. 			float	_21, _22, _23;
  18. 			float	_31, _32, _33;
  19. 		};
  20.  
  21. 		float	m[3][3];
  22. 	};
  23. };
  24.  
  25. void matrix_multiply(struct matrix3_t *out, const struct matrix3_t *m1, const struct matrix3_t *m2)
  26. {
  27. 	int i, j;
  28.  
  29. 	assert(out);
  30. 	assert(m1);
  31. 	assert(m2);
  32.  
  33. 	for (i = 0; i < 3; i++) {
  34. 	    for (j = 0; j < 3; j++) {
  35. 	        out->m[i][j] = 
  36. 	            m1->m[i][0]*m2->m[0][j] +
  37. 	            m1->m[i][1]*m2->m[1][j] +
  38. 	            m1->m[i][2]*m2->m[2][j];
  39. 	    }
  40. 	}
  41.  
  42. }
  43.  
  44. void from_euler_angles_xyz(struct matrix3_t *out, const struct vector3_t *euler)
  45. {
  46. 	float fcos, fsin;
  47. 	float yaw, pitch, roll;
  48. 	struct matrix3_t xm, ym, zm, tmp;
  49.  
  50. 	assert(euler);
  51. 	assert(out);
  52.  
  53. 	yaw = euler->x;
  54. 	pitch = euler->y;
  55. 	roll = euler->z;
  56.  
  57. 	fcos = cosf(yaw);
  58. 	fsin = sinf(yaw);
  59.  
  60. 	xm._11 = 1.0f; xm._12 = 0.0f, xm._13 = 0.0f;
  61. 	xm._21 = 0.0f; xm._22 = fcos, xm._23 = -fsin;
  62. 	xm._31 = 0.0f; xm._32 = fsin, xm._33 = fcos;
  63.  
  64. 	fcos = cosf(pitch);
  65. 	fsin = sinf(pitch);
  66.  
  67. 	ym._11 = fcos; ym._12 = 0.0f, ym._13 = fsin;
  68. 	ym._21 = 0.0f; ym._22 = 1.0f, ym._23 = 0.0f;
  69. 	ym._31 = -fsin; ym._32 = 0.0f, ym._33 = fcos;
  70.  
  71. 	fcos = cosf(roll);
  72. 	fsin = sinf(roll);
  73.  
  74. 	zm._11 = fcos; zm._12 = -fsin, zm._13 = 0.0f;
  75. 	zm._21 = fsin; zm._22 = fcos, zm._23 = 0.0f;
  76. 	zm._31 = 0.0f; zm._32 = 0.0f, zm._33 = 1.0f;
  77.  
  78. 	matrix_multiply(&tmp, &ym, &zm);
  79. 	matrix_multiply(out, &xm, &tmp);
  80. }
  81.  
  82. void vector_transform(struct vector3_t *out, const struct vector3_t *v, const struct matrix3_t *rm)
  83. {
  84. 	int i;
  85. 	float *ptr;
  86.  
  87. 	assert(out);
  88. 	assert(v);
  89. 	assert(rm);
  90.  
  91. 	ptr = (float*) &out->x;
  92. 	for (i = 0; i < 3; i++) {
  93. 	    ptr[i] =
  94. 	        v->x * rm->m[0][i] +
  95. 	        v->y * rm->m[1][i] +
  96. 	        v->z * rm->m[2][i];
  97. 	}
  98. }
  99.  
  100. int main(void)
  101. {
  102. 	struct vector3_t source;
  103. 	struct vector3_t euler;
  104. 	struct vector3_t result;
  105. 	struct matrix3_t rotations;
  106.  
  107. 	source.x = 1.0f;
  108. 	source.y = 1.0f;
  109. 	source.z = 1.0f;
  110.  
  111. 	euler.x = 3.1415f;
  112. 	euler.y = 0.0f;
  113. 	euler.z = 0.0f;
  114.  
  115. 	from_euler_angles_xyz(&rotations, &euler);
  116. 	vector_transform(&result, &source, &rotations);
  117.  
  118. 	printf("rotated vector is: x=%f, y=%f, z=%f\n", result.x, result.y, result.z);
  119. 	puts("press enter...");
  120.  
  121. 	getchar();
  122.  
  123.     return 0;
  124. }
Success #stdin #stdout 0.01s 1680KB
comments (?)
stdin 
Standard input is empty
stdout 
rotated vector is: x=1.000000, y=-0.999907, z=-1.000093
press enter...
https://ideone.com/rwNId
language:
C (gcc 8.3)
created:
11 years ago
visibility:
public

Share or Embed source code

Discover > Sphere Engine API

The brand new service which powers Ideone!

Discover > IDE Widget

Widget for compiling and running the source code in a web browser!