Transform.h 8.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383
  1. #ifndef TRANSFORM_H
  2. #define TRANSFORM_H
  3. #include "Basis.h"
  4. #include "Plane.h"
  5. #include "Rect3.h"
  6. namespace godot {
  7. class Transform {
  8. public:
  9. Basis basis;
  10. Vector3 origin;
  11. void invert();
  12. Transform inverse() const;
  13. void affine_invert();
  14. Transform affine_inverse() const;
  15. Transform rotated(const Vector3& p_axis,real_t p_phi) const;
  16. void rotate(const Vector3& p_axis,real_t p_phi);
  17. void rotate_basis(const Vector3& p_axis,real_t p_phi);
  18. void set_look_at( const Vector3& p_eye, const Vector3& p_target, const Vector3& p_up );
  19. Transform looking_at( const Vector3& p_target, const Vector3& p_up ) const;
  20. void scale(const Vector3& p_scale);
  21. Transform scaled(const Vector3& p_scale) const;
  22. void scale_basis(const Vector3& p_scale);
  23. void translate( real_t p_tx, real_t p_ty, real_t p_tz );
  24. void translate( const Vector3& p_translation );
  25. Transform translated( const Vector3& p_translation ) const;
  26. const Basis& get_basis() const { return basis; }
  27. void set_basis(const Basis& p_basis) { basis=p_basis; }
  28. const Vector3& get_origin() const { return origin; }
  29. void set_origin(const Vector3& p_origin) { origin=p_origin; }
  30. void orthonormalize();
  31. Transform orthonormalized() const;
  32. bool operator==(const Transform& p_transform) const;
  33. bool operator!=(const Transform& p_transform) const;
  34. Vector3 xform(const Vector3& p_vector) const;
  35. Vector3 xform_inv(const Vector3& p_vector) const;
  36. Plane xform(const Plane& p_plane) const;
  37. Plane xform_inv(const Plane& p_plane) const;
  38. Rect3 xform(const Rect3& p_aabb) const;
  39. Rect3 xform_inv(const Rect3& p_aabb) const;
  40. void operator*=(const Transform& p_transform);
  41. Transform operator*(const Transform& p_transform) const;
  42. Transform interpolate_with(const Transform& p_transform, real_t p_c) const;
  43. Transform inverse_xform(const Transform& t) const {
  44. Vector3 v = t.origin - origin;
  45. return Transform(basis.transpose_xform(t.basis),
  46. basis.xform(v));
  47. }
  48. void set(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz,real_t tx, real_t ty, real_t tz) {
  49. basis.elements[0][0]=xx;
  50. basis.elements[0][1]=xy;
  51. basis.elements[0][2]=xz;
  52. basis.elements[1][0]=yx;
  53. basis.elements[1][1]=yy;
  54. basis.elements[1][2]=yz;
  55. basis.elements[2][0]=zx;
  56. basis.elements[2][1]=zy;
  57. basis.elements[2][2]=zz;
  58. origin.x=tx;
  59. origin.y=ty;
  60. origin.z=tz;
  61. }
  62. operator String() const;
  63. Transform(const Basis& p_basis, const Vector3& p_origin=Vector3());
  64. Transform() {}
  65. };
  66. Vector3 Transform::xform(const Vector3& p_vector) const {
  67. return Vector3(
  68. basis[0].dot(p_vector)+origin.x,
  69. basis[1].dot(p_vector)+origin.y,
  70. basis[2].dot(p_vector)+origin.z
  71. );
  72. }
  73. Vector3 Transform::xform_inv(const Vector3& p_vector) const {
  74. Vector3 v = p_vector - origin;
  75. return Vector3(
  76. (basis.elements[0][0]*v.x ) + ( basis.elements[1][0]*v.y ) + ( basis.elements[2][0]*v.z ),
  77. (basis.elements[0][1]*v.x ) + ( basis.elements[1][1]*v.y ) + ( basis.elements[2][1]*v.z ),
  78. (basis.elements[0][2]*v.x ) + ( basis.elements[1][2]*v.y ) + ( basis.elements[2][2]*v.z )
  79. );
  80. }
  81. Plane Transform::xform(const Plane& p_plane) const {
  82. Vector3 point=p_plane.normal*p_plane.d;
  83. Vector3 point_dir=point+p_plane.normal;
  84. point=xform(point);
  85. point_dir=xform(point_dir);
  86. Vector3 normal=point_dir-point;
  87. normal.normalize();
  88. real_t d=normal.dot(point);
  89. return Plane(normal,d);
  90. }
  91. Plane Transform::xform_inv(const Plane& p_plane) const {
  92. Vector3 point=p_plane.normal*p_plane.d;
  93. Vector3 point_dir=point+p_plane.normal;
  94. xform_inv(point);
  95. xform_inv(point_dir);
  96. Vector3 normal=point_dir-point;
  97. normal.normalize();
  98. real_t d=normal.dot(point);
  99. return Plane(normal,d);
  100. }
  101. Rect3 Transform::xform(const Rect3& p_aabb) const {
  102. /* define vertices */
  103. Vector3 x=basis.get_axis(0)*p_aabb.size.x;
  104. Vector3 y=basis.get_axis(1)*p_aabb.size.y;
  105. Vector3 z=basis.get_axis(2)*p_aabb.size.z;
  106. Vector3 pos = xform( p_aabb.pos );
  107. //could be even further optimized
  108. Rect3 new_aabb;
  109. new_aabb.pos=pos;
  110. new_aabb.expand_to( pos+x );
  111. new_aabb.expand_to( pos+y );
  112. new_aabb.expand_to( pos+z );
  113. new_aabb.expand_to( pos+x+y );
  114. new_aabb.expand_to( pos+x+z );
  115. new_aabb.expand_to( pos+y+z );
  116. new_aabb.expand_to( pos+x+y+z );
  117. return new_aabb;
  118. }
  119. Rect3 Transform::xform_inv(const Rect3& p_aabb) const {
  120. /* define vertices */
  121. Vector3 vertices[8]={
  122. Vector3(p_aabb.pos.x+p_aabb.size.x, p_aabb.pos.y+p_aabb.size.y, p_aabb.pos.z+p_aabb.size.z),
  123. Vector3(p_aabb.pos.x+p_aabb.size.x, p_aabb.pos.y+p_aabb.size.y, p_aabb.pos.z),
  124. Vector3(p_aabb.pos.x+p_aabb.size.x, p_aabb.pos.y, p_aabb.pos.z+p_aabb.size.z),
  125. Vector3(p_aabb.pos.x+p_aabb.size.x, p_aabb.pos.y, p_aabb.pos.z),
  126. Vector3(p_aabb.pos.x, p_aabb.pos.y+p_aabb.size.y, p_aabb.pos.z+p_aabb.size.z),
  127. Vector3(p_aabb.pos.x, p_aabb.pos.y+p_aabb.size.y, p_aabb.pos.z),
  128. Vector3(p_aabb.pos.x, p_aabb.pos.y, p_aabb.pos.z+p_aabb.size.z),
  129. Vector3(p_aabb.pos.x, p_aabb.pos.y, p_aabb.pos.z)
  130. };
  131. Rect3 ret;
  132. ret.pos=xform_inv(vertices[0]);
  133. for (int i=1;i<8;i++) {
  134. ret.expand_to( xform_inv(vertices[i]) );
  135. }
  136. return ret;
  137. }
  138. }
  139. namespace godot {
  140. void Transform::affine_invert() {
  141. basis.invert();
  142. origin = basis.xform(-origin);
  143. }
  144. Transform Transform::affine_inverse() const {
  145. Transform ret=*this;
  146. ret.affine_invert();
  147. return ret;
  148. }
  149. void Transform::invert() {
  150. basis.transpose();
  151. origin = basis.xform(-origin);
  152. }
  153. Transform Transform::inverse() const {
  154. // FIXME: this function assumes the basis is a rotation matrix, with no scaling.
  155. // Transform::affine_inverse can handle matrices with scaling, so GDScript should eventually use that.
  156. Transform ret=*this;
  157. ret.invert();
  158. return ret;
  159. }
  160. void Transform::rotate(const Vector3& p_axis,real_t p_phi) {
  161. *this = rotated(p_axis, p_phi);
  162. }
  163. Transform Transform::rotated(const Vector3& p_axis,real_t p_phi) const{
  164. return Transform(Basis( p_axis, p_phi ), Vector3()) * (*this);
  165. }
  166. void Transform::rotate_basis(const Vector3& p_axis,real_t p_phi) {
  167. basis.rotate(p_axis,p_phi);
  168. }
  169. Transform Transform::looking_at( const Vector3& p_target, const Vector3& p_up ) const {
  170. Transform t = *this;
  171. t.set_look_at(origin,p_target,p_up);
  172. return t;
  173. }
  174. void Transform::set_look_at( const Vector3& p_eye, const Vector3& p_target, const Vector3& p_up ) {
  175. // Reference: MESA source code
  176. Vector3 v_x, v_y, v_z;
  177. /* Make rotation matrix */
  178. /* Z vector */
  179. v_z = p_eye - p_target;
  180. v_z.normalize();
  181. v_y = p_up;
  182. v_x=v_y.cross(v_z);
  183. /* Recompute Y = Z cross X */
  184. v_y=v_z.cross(v_x);
  185. v_x.normalize();
  186. v_y.normalize();
  187. basis.set_axis(0,v_x);
  188. basis.set_axis(1,v_y);
  189. basis.set_axis(2,v_z);
  190. origin=p_eye;
  191. }
  192. Transform Transform::interpolate_with(const Transform& p_transform, real_t p_c) const {
  193. /* not sure if very "efficient" but good enough? */
  194. Vector3 src_scale = basis.get_scale();
  195. Quat src_rot = basis;
  196. Vector3 src_loc = origin;
  197. Vector3 dst_scale = p_transform.basis.get_scale();
  198. Quat dst_rot = p_transform.basis;
  199. Vector3 dst_loc = p_transform.origin;
  200. Transform dst;
  201. dst.basis=src_rot.slerp(dst_rot,p_c);
  202. dst.basis.scale(src_scale.linear_interpolate(dst_scale,p_c));
  203. dst.origin=src_loc.linear_interpolate(dst_loc,p_c);
  204. return dst;
  205. }
  206. void Transform::scale(const Vector3& p_scale) {
  207. basis.scale(p_scale);
  208. origin*=p_scale;
  209. }
  210. Transform Transform::scaled(const Vector3& p_scale) const {
  211. Transform t = *this;
  212. t.scale(p_scale);
  213. return t;
  214. }
  215. void Transform::scale_basis(const Vector3& p_scale) {
  216. basis.scale(p_scale);
  217. }
  218. void Transform::translate( real_t p_tx, real_t p_ty, real_t p_tz) {
  219. translate( Vector3(p_tx,p_ty,p_tz) );
  220. }
  221. void Transform::translate( const Vector3& p_translation ) {
  222. for( int i = 0; i < 3; i++ ) {
  223. origin[i] += basis[i].dot(p_translation);
  224. }
  225. }
  226. Transform Transform::translated( const Vector3& p_translation ) const {
  227. Transform t=*this;
  228. t.translate(p_translation);
  229. return t;
  230. }
  231. void Transform::orthonormalize() {
  232. basis.orthonormalize();
  233. }
  234. Transform Transform::orthonormalized() const {
  235. Transform _copy = *this;
  236. _copy.orthonormalize();
  237. return _copy;
  238. }
  239. bool Transform::operator==(const Transform& p_transform) const {
  240. return (basis==p_transform.basis && origin==p_transform.origin);
  241. }
  242. bool Transform::operator!=(const Transform& p_transform) const {
  243. return (basis!=p_transform.basis || origin!=p_transform.origin);
  244. }
  245. void Transform::operator*=(const Transform& p_transform) {
  246. origin=xform(p_transform.origin);
  247. basis*=p_transform.basis;
  248. }
  249. Transform Transform::operator*(const Transform& p_transform) const {
  250. Transform t=*this;
  251. t*=p_transform;
  252. return t;
  253. }
  254. Transform::operator String() const {
  255. return basis.operator String() + " - " + origin.operator String();
  256. }
  257. Transform::Transform(const Basis& p_basis, const Vector3& p_origin) {
  258. basis=p_basis;
  259. origin=p_origin;
  260. }
  261. }
  262. #endif // TRANSFORM_H