diff --git a/legacy/ephysics/src/bin/ephysics_sandbox.c b/legacy/ephysics/src/bin/ephysics_sandbox.c index 4d1a4e64b7..5d29238568 100644 --- a/legacy/ephysics/src/bin/ephysics_sandbox.c +++ b/legacy/ephysics/src/bin/ephysics_sandbox.c @@ -115,16 +115,17 @@ _world_restitution_cb(void *data, Evas_Object *obj, void *event_info __UNUSED__) static void _type_set_cb(void *data, Evas_Object *obj, void *event_info __UNUSED__) { - double mass, rotation, friction, restitution, lin_damping, ang_damping; + double mass, friction, restitution, lin_damping, ang_damping; double lin_sleeping, ang_sleeping; EPhysics_Body_Material material; + EPhysics_Quaternion *rotation; Evas_Object *body_image; EPhysics_World *world; Body_Data *bd = data; EPhysics_Body *body = bd->body; mass = ephysics_body_mass_get(body); - ephysics_body_rotation_get(body, 0, 0, &rotation); + rotation = ephysics_body_rotation_get(body); friction = ephysics_body_friction_get(body); restitution = ephysics_body_restitution_get(body); ephysics_body_damping_get(body, &lin_damping, &ang_damping); @@ -151,7 +152,7 @@ _type_set_cb(void *data, Evas_Object *obj, void *event_info __UNUSED__) ephysics_body_evas_object_set(body, body_image, EINA_TRUE); ephysics_body_mass_set(body, mass); - ephysics_body_rotation_set(body, 0, 0, rotation); + ephysics_body_rotation_set(body, rotation); ephysics_body_friction_set(body, friction); ephysics_body_restitution_set(body, restitution); ephysics_body_damping_set(body, lin_damping, ang_damping); @@ -167,6 +168,7 @@ _type_set_cb(void *data, Evas_Object *obj, void *event_info __UNUSED__) elm_slider_value_get(bd->controls.force.torque)); bd->body = body; + free(rotation); } static void @@ -227,8 +229,13 @@ _density_set_cb(void *data, Evas_Object *obj, void *event_info __UNUSED__) static void _rotation_set_cb(void *data, Evas_Object *obj, void *event_info __UNUSED__) { + EPhysics_Quaternion *quat; Body_Data *bd = data; - ephysics_body_rotation_set(bd->body, 0, 0, elm_slider_value_get(obj)); + + quat = ephysics_quaternion_new(0, 0, 0, 0); + ephysics_quaternion_euler_set(quat, 0, 0, elm_slider_value_get(obj)); + ephysics_body_rotation_set(bd->body, quat); + free(quat); } static void @@ -713,7 +720,7 @@ _menu_body_page_add(World_Data *wd, Body_Data *bd, const char *pg_label) evas_object_data_set(widget, "mass", aux_widget); evas_object_smart_callback_add(widget, "delay,changed", _density_set_cb, bd); - widget = _slider_add(bx, "Rotation (ยบ)", "%1.0f", 0, 360, 0); + widget = _slider_add(bx, "Rotation (rad)", "%1.3f", -3.1415, 3.1415, 0); evas_object_smart_callback_add(widget, "delay,changed", _rotation_set_cb, bd); widget = _slider_add(bx, "Friction", "%1.3f", 0, 1, 0.5); diff --git a/legacy/ephysics/src/bin/test_bouncing_3d.c b/legacy/ephysics/src/bin/test_bouncing_3d.c index 2012b70cb8..e239ab699f 100644 --- a/legacy/ephysics/src/bin/test_bouncing_3d.c +++ b/legacy/ephysics/src/bin/test_bouncing_3d.c @@ -7,14 +7,18 @@ static void _pos_print_cb(void *data __UNUSED__, EPhysics_Body *body, void *event_info __UNUSED__) { + EPhysics_Quaternion *quat; + double rx, ry, rz, rw; Evas_Coord x, y, z; - double rx, ry, rz; ephysics_body_geometry_get(body, &x, &y, &z, NULL, NULL, NULL); - ephysics_body_rotation_get(body, &rx, &ry, &rz); + + quat = ephysics_body_rotation_get(body); + ephysics_quaternion_get(quat, &rx, &ry, &rz, &rw); + free(quat); printf("Position X:%i Y:%i Z:%i\n", x, y, z); - printf("Rotation X:%lf Y:%lf Z:%lf\n", rx, ry, rz); + printf("Rotation X:%lf Y:%lf Z:%lf W:%lf\n", rx, ry, rz, rw); } static Eina_Bool diff --git a/legacy/ephysics/src/bin/test_cube_3d.c b/legacy/ephysics/src/bin/test_cube_3d.c index 01bcf7f8bb..cf107299b7 100644 --- a/legacy/ephysics/src/bin/test_cube_3d.c +++ b/legacy/ephysics/src/bin/test_cube_3d.c @@ -9,16 +9,25 @@ static void _mouse_move_cb(void *data, Evas *e __UNUSED__, Evas_Object *obj __UNUSED__, void *event_info) { + EPhysics_Quaternion *quat_prev, *quat_delta, *quat; Evas_Event_Mouse_Move *mmove = event_info; EPhysics_Body *body = data; - double rx, ry, rz; + double rx, ry; if (mmove->buttons != 1) return; - ephysics_body_rotation_get(body, &rx, &ry, &rz); - rx += mmove->cur.output.y - mmove->prev.output.y; - ry += mmove->cur.output.x - mmove->prev.output.x; - ephysics_body_rotation_set(body, rx, ry, rz); + rx = mmove->cur.output.y - mmove->prev.output.y; + ry = mmove->cur.output.x - mmove->prev.output.x; + + quat_prev = ephysics_body_rotation_get(body); + quat_delta = ephysics_quaternion_new(0, 0, 0, 0); + ephysics_quaternion_euler_set(quat_delta, -ry * 0.06, - rx * 0.04, 0); + quat = ephysics_quaternion_multiply(quat_prev, quat_delta); + ephysics_body_rotation_set(body, quat); + + free(quat_prev); + free(quat_delta); + free(quat); } static void diff --git a/legacy/ephysics/src/bin/test_rotating_forever.c b/legacy/ephysics/src/bin/test_rotating_forever.c index 6afc68339e..d0be41f939 100644 --- a/legacy/ephysics/src/bin/test_rotating_forever.c +++ b/legacy/ephysics/src/bin/test_rotating_forever.c @@ -7,11 +7,19 @@ static Eina_Bool _rotate_cb(void *data) { + EPhysics_Quaternion *quat_prev, *quat_delta, *quat; EPhysics_Body *body = data; - double rotation; - ephysics_body_rotation_get(body, NULL, NULL, &rotation); - ephysics_body_rotation_set(body, 0, 0, ((int) round(rotation) + 5) % 360); + quat_prev = ephysics_body_rotation_get(body); + quat_delta = ephysics_quaternion_new(0, 0, -0.15, 0.98); + ephysics_quaternion_normalize(quat_delta); + quat = ephysics_quaternion_multiply(quat_delta, quat_prev); + + ephysics_body_rotation_set(body, quat); + + free(quat_prev); + free(quat_delta); + free(quat); return EINA_TRUE; } @@ -65,15 +73,20 @@ _del_torque_cb(void *data __UNUSED__, Evas *e __UNUSED__, Evas_Object *obj, void static void _update_object_cb(void *data __UNUSED__, EPhysics_Body *body, void *event_info __UNUSED__) { - double rot, vrot, torque; + double rx, ry, rz, rw, vrot, torque; + EPhysics_Quaternion *quat; - ephysics_body_rotation_get(body, NULL, NULL, &rot); ephysics_body_angular_velocity_get(body, NULL, NULL, &vrot); ephysics_body_torques_get(body, NULL, NULL, &torque); + quat = ephysics_body_rotation_get(body); + ephysics_quaternion_get(quat, &rx, &ry, &rz, &rw); + free(quat); + ephysics_body_evas_object_update(body); - DBG("body: %p, rot: %lf, vrot: %lf, torque: %lf", body, rot, vrot, torque); + DBG("body: %p, rot: (%lf, %lf, %lf, %lf), vrot: %lf, torque: %lf", body, + rx, ry, rz, rw, vrot, torque); } static void diff --git a/legacy/ephysics/src/lib/EPhysics.h b/legacy/ephysics/src/lib/EPhysics.h index a4409a34d1..df2b3da0da 100644 --- a/legacy/ephysics/src/lib/EPhysics.h +++ b/legacy/ephysics/src/lib/EPhysics.h @@ -115,6 +115,319 @@ EAPI int ephysics_init(void); */ EAPI int ephysics_shutdown(void); +/** + * @} + */ + +/** + * @defgroup EPhysics_Quaternion EPhysics Quaternion + * @ingroup EPhysics + * + * @{ + * + * Quaternions are used to perform linear algebra rotations. + * + * Functions regarding rotation, like @ref ephysics_body_rotation_set() + * and @ref ephysics_body_rotation_get() would need that. Quaternions + * can be used to rotate evas maps as well, with evas_map_util_quat_rotate(), + * but in this case quaternion values need to be get with + * @ref ephysics_quaternion_get(), since evas don't accept + * EPhysics_Quaternion type. + * + * A quaternion can be created with ephysics_quaternion_new(), and many + * operations can be performed with that, as: + * @li Sum: @ref ephysics_quaternion_sum() + * @li Difference: @ref ephysics_quaternion_diff() + * @li Multiple by another quaternion: @ref ephysics_quaternion_multiply() + * @li Multiply by scalar: @ref ephysics_quaternion_scale() + * @li Divide by scalar: @ref ephysics_quaternion_inverse_scale() + * @li Calculate length: @ref ephysics_quaternion_length_get() + * @li Calculate angle between quaternions: @ref ephysics_quaternion_angle_get() + */ + +/** + * @typedef EPhysics_Quaternion + * + * Quaternion handle, represents a quaternion to be used to rotate bodies. + * + * Created with @ref ephysics_quaternion_new() and deleted with free(). + * + * @ingroup EPhysics_Quaternion + */ +typedef struct _EPhysics_Quaternion EPhysics_Quaternion; + +/** + * @brief + * Create a new quaternion. + * + * @note It should be deleted with free() after usage is concluded. + * + * This values can be modified later by quaternion operations or set directly. + * + * @param x The x coordinate. + * @param y The y coordinate. + * @param z The z coordinate. + * @param w The rotation. + * @return The created quaternion or @c NULL on error. + * + * @see ephysics_quaternion_set(); + * @see ephysics_quaternion_axis_angle_set(); + * @see ephysics_quaternion_euler_set(); + * @see ephysics_quaternion_scale(); + * @see ephysics_quaternion_sum(); + * + * @ingroup EPhysics_Quaternion + */ +EAPI EPhysics_Quaternion *ephysics_quaternion_new(double x, double y, double z, double w); + +/** + * @brief + * Get quaternion values. + * + * @param quat Quaternion to get values from. + * @param x The x coordinate. + * @param y The y coordinate. + * @param z The z coordinate. + * @param w The rotation. + * + * @see ephysics_quaternion_set(); + * + * @ingroup EPhysics_Quaternion + */ +EAPI void ephysics_quaternion_get(const EPhysics_Quaternion *quat, double *x, double *y, double *z, double *w); + +/** + * @brief + * Get quaternion axis and angle. + * + * @param quat Quaternion to get values from. + * @param nx The x component of the axis of rotation. + * @param ny The y component of the axis of rotation. + * @param nz The z component of the axis of rotation. + * @param a The angle of rotation. + * + * @see ephysics_quaternion_axis_angle_set(); + * @see ephysics_quaternion_get(); + * @see ephysics_quaternion_set(); + * + * @ingroup EPhysics_Quaternion + */ +EAPI void ephysics_quaternion_axis_angle_get(const EPhysics_Quaternion *quat, double *nx, double *ny, double *nz, double *a); + +/** + * @brief + * Set quaternion values. + * + * @param quat Quaternion to be set. + * @param x The x coordinate. + * @param y The y coordinate. + * @param z The z coordinate. + * @param w The rotation. + * + * @see ephysics_quaternion_get(); + * @see ephysics_quaternion_euler_set(); + * + * @ingroup EPhysics_Quaternion + */ +EAPI void ephysics_quaternion_set(EPhysics_Quaternion *quat, double x, double y, double z, double w); + +/** + * @brief + * Set quaternion using axis angle notation. + * + * [w, x, y, z] = [cos(a/2), sin(a/2) * nx, sin(a/2)* ny, sin(a/2) * nz] + * + * @param quat Quaternion to be set. + * @param nx The x component of the axis of rotation. + * @param ny The y component of the axis of rotation. + * @param nz The z component of the axis of rotation. + * @param a The angle of rotation. + * + * @see ephysics_quaternion_axis_angle_get(); + * @see ephysics_quaternion_set(); + * @see ephysics_quaternion_euler_set(); + * + * @ingroup EPhysics_Quaternion + */ +EAPI void ephysics_quaternion_axis_angle_set(EPhysics_Quaternion *quat, double nx, double ny, double nz, double a); + +/** + * @brief + * Set quaternion using Euler angles. + * + * It's an alternative to @ref ephysics_quaternion_set() usage. Euler angles + * will be converted. + * + * @param quat Quaternion to be set. + * @param yaw The angle around Y axis. + * @param pitch The angle around X axis. + * @param roll The angle around Z axis. + * + * @see ephysics_quaternion_get(); + * @see ephysics_quaternion_set(); + * + * @ingroup EPhysics_Quaternion + */ +EAPI void ephysics_quaternion_euler_set(EPhysics_Quaternion *quat, double yaw, double pitch, double roll); + +/** + * @brief + * Normalize the quaternion. + * + * A normalized quaternion is such that x^2 + y^2 + z^2 + w^2 = 1. + * + * @param quat Quaternion to be normalized. + * + * @ingroup EPhysics_Quaternion + */ +EAPI void ephysics_quaternion_normalize(EPhysics_Quaternion *quat); + +/** + * @brief + * Invert the quaternion. + * + * @param quat Quaternion to be inverted. + * + * @ingroup EPhysics_Quaternion + */ +EAPI void ephysics_quaternion_invert(EPhysics_Quaternion *quat); + +/** + * @brief + * Scale the quaternion. + * + * @param quat Quaternion to be scaled. + * @param scale The scale factor. + * + * @see ephysics_quaternion_inverse_scale() + * + * @ingroup EPhysics_Quaternion + */ +EAPI void ephysics_quaternion_scale(EPhysics_Quaternion *quat, double scale); + +/** + * @brief + * Inversely scale the quaternion. + * + * @param quat Quaternion to be scaled. + * @param scale The scale factor. + * + * @see ephysics_quaternion_scale() + * + * @ingroup EPhysics_Quaternion + */ +EAPI void ephysics_quaternion_inverse_scale(EPhysics_Quaternion *quat, double scale); + +/** + * @brief + * Returns a sum of two quaternions. + * + * @param quat1 First quaternion to sum. + * @param quat2 Second quaternion to sum. + * @return The sum quaternion or @c NULL on error. + * + * @note It should be freed after usage. + * + * @ingroup EPhysics_Quaternion + */ +EAPI EPhysics_Quaternion *ephysics_quaternion_sum(const EPhysics_Quaternion *quat1, const EPhysics_Quaternion *quat2); + +/** + * @brief + * Returns a difference between two quaternions. + * + * @param quat1 First quaternion. + * @param quat2 Second quaternion. + * @return The difference between @p quat1 and @p quat2, or @c NULL on error. + * + * @note It should be freed after usage. + * + * @ingroup EPhysics_Quaternion + */ +EAPI EPhysics_Quaternion *ephysics_quaternion_diff(const EPhysics_Quaternion *quat1, const EPhysics_Quaternion *quat2); + +/** + * @brief + * Multiply two quaternions. + * + * @param quat1 First quaternion. + * @param quat2 Second quaternion. + * @return The @p quat1 multiplied by @p quat2 on the right, or @c NULL + * on error. + * + * @note It should be freed after usage. + * + * @ingroup EPhysics_Quaternion + */ +EAPI EPhysics_Quaternion *ephysics_quaternion_multiply(const EPhysics_Quaternion *quat1, const EPhysics_Quaternion *quat2); + +/** + * @brief + * Return the quaternion which is the result of Spherical Linear Interpolation + * between two quaternions. + * + * Slerp interpolates assuming constant velocity. + * + * @param quat1 First quaternion. + * @param quat2 Second quaternion. + * @param ratio The ratio between @p quat1 and @p quat2 to interpolate. If + * @p ratio = 0, the result is @p quat1, if @p ratio = 1, the result is + * @p quat2. + * @return The result of slerp between @p quat1 and @p quat2, or @c NULL + * on error. + * + * @note It should be freed after usage. + * + * @ingroup EPhysics_Quaternion + */ +EAPI EPhysics_Quaternion *ephysics_quaternion_slerp(const EPhysics_Quaternion *quat1, const EPhysics_Quaternion *quat2, double ratio); + +/** + * @brief + * Return the dot product between two quaternions. + * + * @param quat1 First quaternion. + * @param quat2 Second quaternion. + * @return The dot product between @p quat1 and @p quat2 or @c 0 on error. + * + * @ingroup EPhysics_Quaternion + */ +EAPI double ephysics_quaternion_dot(const EPhysics_Quaternion *quat1, const EPhysics_Quaternion *quat2); + +/** + * @brief + * Return the angle between two quaternions. + * + * @param quat1 First quaternion. + * @param quat2 Second quaternion. + * @return The angle between @p quat1 and @p quat2 or @c 0 on error. + * + * @ingroup EPhysics_Quaternion + */ +EAPI double ephysics_quaternion_angle_get(const EPhysics_Quaternion *quat1, const EPhysics_Quaternion *quat2); + +/** + * @brief + * Return the length of the quaternion. + * + * @param quat Quaternion to get length of. + * @return The lenght of @p quat or @c 0 on error. + * + * @ingroup EPhysics_Quaternion + */ +EAPI double ephysics_quaternion_length_get(const EPhysics_Quaternion *quat); + +/** + * @brief + * Return the length squared of the quaternion. + * + * @param quat Quaternion to get length of. + * @return The lenght of @p quat or @c 0 on error. + * + * @ingroup EPhysics_Quaternion + */ +EAPI double ephysics_quaternion_length2_get(const EPhysics_Quaternion *quat); + /** * @} */ @@ -3422,61 +3735,36 @@ EAPI void ephysics_body_linear_movement_enable_get(const EPhysics_Body *body, Ei /** * @brief - * Get body's rotation on x, y and z axes. + * Get body's rotation quaternion. * * By default rotation is 0 degree on all axes. * - * @note The unit used for rotation is degrees. - * * @param body The physics body. - * @param rot_x The amount of degrees @p body is rotated on x axis. - * @param rot_y The amount of degrees @p body is rotated on y axis. - * @param rot_z The amount of degrees @p body is rotated on z axis. + * @return A quaternion or @c NULL on error. It should be freed with free() + * after usage. * * @see ephysics_body_rotation_set() - * @see ephysics_body_rotation_quaternion_get() + * @see ephysics_quaternion_get() * * @ingroup EPhysics_Body */ -EAPI void ephysics_body_rotation_get(const EPhysics_Body *body, double *rot_x, double *rot_y, double *rot_z); +EAPI EPhysics_Quaternion *ephysics_body_rotation_get(const EPhysics_Body *body); /** * @brief - * Set body's rotation on z axis. + * Set body's rotation. * * By default rotation is 0 degrees on all axes. - * Negative values indicates rotation on counter clockwise direction. - * - * @note The unit used for rotation is degrees. * * @param body The physics body. - * @param rot_x The amount of degrees @p body should be rotated on x axis. - * @param rot_y The amount of degrees @p body should be rotated on y axis. - * @param rot_z The amount of degrees @p body should be rotated on z axis. + * @param quat Quaternion representing the rotation. * * @see ephysics_body_rotation_get() + * @see ephysics_quaternion_new() * * @ingroup EPhysics_Body */ -EAPI void ephysics_body_rotation_set(EPhysics_Body *body, double rot_x, double rot_y, double rot_z); - -/** - * @brief - * Get body's normalized rotation quaternion (x, y, z and w). - * - * @param body The physics body. - * @param x the x component of the imaginary part of the quaternion. - * @param y the y component of the imaginary part of the quaternion. - * @param z the z component of the imaginary part of the quaternion. - * @param w the w component of the real part of the quaternion. - * - * @see ephysics_body_rotation_set() - * @see ephysics_body_rotation_get() - * - * @ingroup EPhysics_Body - */ -EAPI void -ephysics_body_rotation_quaternion_get(const EPhysics_Body *body, double *x, double *y, double *z, double *w); +EAPI void ephysics_body_rotation_set(EPhysics_Body *body, EPhysics_Quaternion *quat); /** * @brief diff --git a/legacy/ephysics/src/lib/Makefile.am b/legacy/ephysics/src/lib/Makefile.am index c91e5fb786..071b6fbc91 100644 --- a/legacy/ephysics/src/lib/Makefile.am +++ b/legacy/ephysics/src/lib/Makefile.am @@ -18,6 +18,7 @@ base_sources = \ ephysics_camera.cpp \ ephysics_constraints.cpp \ ephysics_main.cpp \ + ephysics_quaternion.cpp \ ephysics_shape.cpp \ ephysics_world.cpp diff --git a/legacy/ephysics/src/lib/ephysics_body.cpp b/legacy/ephysics/src/lib/ephysics_body.cpp index 472ce80099..2054e49528 100644 --- a/legacy/ephysics/src/lib/ephysics_body.cpp +++ b/legacy/ephysics/src/lib/ephysics_body.cpp @@ -21,6 +21,7 @@ typedef struct _EPhysics_Body_Callback EPhysics_Body_Callback; typedef struct _EPhysics_Body_Evas_Stacking EPhysics_Body_Evas_Stacking; typedef struct _EPhysics_Body_Soft_Body_Slice EPhysics_Body_Soft_Body_Slice; typedef struct _EPhysics_Body_Face_Obj EPhysics_Body_Face_Obj; +typedef struct _EPhysics_Quaternion EPhysics_Quaternion; struct _EPhysics_Body_Callback { EINA_INLIST; @@ -3266,31 +3267,32 @@ ephysics_body_angular_movement_enable_get(const EPhysics_Body *body, Eina_Bool * if (enable_z) *enable_z = !!body->rigid_body->getAngularFactor().z(); } -EAPI void -ephysics_body_rotation_get(const EPhysics_Body *body, double *rot_x, double *rot_y, double *rot_z) +EAPI EPhysics_Quaternion * +ephysics_body_rotation_get(const EPhysics_Body *body) { - btScalar yaw, pitch, roll; + EPhysics_Quaternion *quat; btTransform trans; if (!body) { ERR("Can't get rotation, body is null."); - return; + return NULL; } trans = _ephysics_body_transform_get(body); - - trans.getBasis().getEulerYPR(yaw, pitch, roll); - if (rot_x) *rot_x = -roll * RAD_TO_DEG; - if (rot_y) *rot_y = -pitch * RAD_TO_DEG; - if (rot_z) *rot_z = -yaw * RAD_TO_DEG; + quat = ephysics_quaternion_new(trans.getRotation().x(), + trans.getRotation().y(), + trans.getRotation().z(), + trans.getRotation().getW()); + return quat; } EAPI void -ephysics_body_rotation_set(EPhysics_Body *body, double rot_x, double rot_y, double rot_z) +ephysics_body_rotation_set(EPhysics_Body *body, EPhysics_Quaternion *quat) { + btQuaternion bt_quat; btTransform trans; - btQuaternion quat; + double x, y, z, w; if (!body) { @@ -3298,47 +3300,32 @@ ephysics_body_rotation_set(EPhysics_Body *body, double rot_x, double rot_y, doub return; } + if (!quat) + { + ERR("Can't set rotation, quaternion is null."); + return; + } + + ephysics_quaternion_get(quat, &x, &y, &z, &w); + ephysics_world_lock_take(body->world); ephysics_body_activate(body, EINA_TRUE); - quat.setEuler(-rot_y / RAD_TO_DEG, -rot_x / RAD_TO_DEG, -rot_z / RAD_TO_DEG); + bt_quat = btQuaternion(x, y, z, w); if (body->soft_body) - body->soft_body->rotate(quat); + body->soft_body->rotate(bt_quat); else { trans = _ephysics_body_transform_get(body); - trans.setRotation(quat); + trans.setRotation(bt_quat); body->rigid_body->proceedToTransform(trans); body->rigid_body->getMotionState()->setWorldTransform(trans); } - DBG("Body %p rotation set to (%lf, %lf, %lf)", body, rot_x, rot_y, rot_z); ephysics_world_lock_release(body->world); } -EAPI void -ephysics_body_rotation_quaternion_get(const EPhysics_Body *body, double *x, double *y, double *z, double *w) -{ - btTransform trans; - btQuaternion quat; - - if (!body) - { - ERR("Can't get rotation, body is null."); - return; - } - - trans = _ephysics_body_transform_get(body); - quat = trans.getRotation(); - quat.normalize(); - - if (x) *x = quat.x(); - if (y) *y = quat.y(); - if (z) *z = quat.z(); - if (w) *w = -quat.w(); -} - EAPI void ephysics_body_data_set(EPhysics_Body *body, void *data) { diff --git a/legacy/ephysics/src/lib/ephysics_quaternion.cpp b/legacy/ephysics/src/lib/ephysics_quaternion.cpp new file mode 100644 index 0000000000..f008769839 --- /dev/null +++ b/legacy/ephysics/src/lib/ephysics_quaternion.cpp @@ -0,0 +1,336 @@ +#ifdef HAVE_CONFIG_H +# include +#endif + +#include "ephysics_private.h" + +#ifdef __cplusplus +extern "C" { +#endif + +struct _EPhysics_Quaternion { + double x; + double y; + double z; + double w; +}; + +static void +_ephysics_quaternion_update(EPhysics_Quaternion *quat, btQuaternion *bt_quat) +{ + quat->x = bt_quat->x(); + quat->y = bt_quat->y(); + quat->z = bt_quat->z(); + quat->w = bt_quat->getW(); +} + +EAPI EPhysics_Quaternion * +ephysics_quaternion_new(double x, double y, double z, double w) +{ + EPhysics_Quaternion *quat; + + quat = (EPhysics_Quaternion *)calloc(1, sizeof(EPhysics_Quaternion)); + + if (!quat) + { + ERR("Could not allocate ephysics quaternion."); + return NULL; + } + + quat->x = x; + quat->y = y; + quat->z = z; + quat->w = w; + + return quat; +} + +EAPI void +ephysics_quaternion_get(const EPhysics_Quaternion *quat, double *x, double *y, double *z, double *w) +{ + if (!quat) + { + ERR("Can't get quaternion's values, it is null."); + return; + } + + if (x) *x = quat->x; + if (y) *y = quat->y; + if (z) *z = quat->z; + if (w) *w = quat->w; +} + +EAPI void +ephysics_quaternion_axis_angle_get(const EPhysics_Quaternion *quat, double *nx, double *ny, double *nz, double *a) +{ + btQuaternion bt_quat; + + if (!quat) + { + ERR("Can't get quaternion's values, it is null."); + return; + } + + bt_quat = btQuaternion(quat->x, quat->y, quat->z, quat->w); + + if (nx) *nx = bt_quat.getAxis().getX(); + if (ny) *ny = bt_quat.getAxis().getY(); + if (nz) *nz = bt_quat.getAxis().getZ(); + if (a) *a = bt_quat.getAngle(); +} + +EAPI void +ephysics_quaternion_set(EPhysics_Quaternion *quat, double x, double y, double z, double w) +{ + if (!quat) + { + ERR("Could not set a null quaternion."); + return; + } + + quat->x = x; + quat->y = y; + quat->z = z; + quat->w = w; +} + +EAPI void +ephysics_quaternion_axis_angle_set(EPhysics_Quaternion *quat, double nx, double ny, double nz, double a) +{ + btQuaternion bt_quat; + btVector3 axis; + + if (!quat) + { + ERR("Could not set a null quaternion."); + return; + } + + axis = btVector3(nx, ny, nz); + bt_quat = btQuaternion(axis, a); + _ephysics_quaternion_update(quat, &bt_quat); +} + +EAPI void +ephysics_quaternion_euler_set(EPhysics_Quaternion *quat, double yaw, double pitch, double roll) +{ + btQuaternion bt_quat; + + if (!quat) + { + ERR("Could not set a null quaternion."); + return; + } + + bt_quat = btQuaternion(); + bt_quat.setEuler(yaw, pitch, roll); + _ephysics_quaternion_update(quat, &bt_quat); +} + +EAPI void +ephysics_quaternion_normalize(EPhysics_Quaternion *quat) +{ + btQuaternion bt_quat; + + if (!quat) + { + ERR("Can't normalize a null quaternion."); + return; + } + + bt_quat = btQuaternion(quat->x, quat->y, quat->z, quat->w); + bt_quat.normalize(); + _ephysics_quaternion_update(quat, &bt_quat); +} + +EAPI void +ephysics_quaternion_invert(EPhysics_Quaternion *quat) +{ + btQuaternion bt_quat; + + if (!quat) + { + ERR("Can't normalize a null quaternion."); + return; + } + + bt_quat = btQuaternion(quat->x, quat->y, quat->z, quat->w); + bt_quat.inverse(); + _ephysics_quaternion_update(quat, &bt_quat); +} + +EAPI void +ephysics_quaternion_scale(EPhysics_Quaternion *quat, double scale) +{ + btQuaternion bt_quat; + + if (!quat) + { + ERR("Can't operate over a null quaternion."); + return; + } + + bt_quat = btQuaternion(quat->x, quat->y, quat->z, quat->w); + bt_quat *= scale; + _ephysics_quaternion_update(quat, &bt_quat); +} + +EAPI void +ephysics_quaternion_inverse_scale(EPhysics_Quaternion *quat, double scale) +{ + btQuaternion bt_quat; + + if (!quat) + { + ERR("Can't operate over a null quaternion."); + return; + } + + bt_quat = btQuaternion(quat->x, quat->y, quat->z, quat->w); + bt_quat /= scale; + _ephysics_quaternion_update(quat, &bt_quat); +} + +EAPI EPhysics_Quaternion * +ephysics_quaternion_sum(const EPhysics_Quaternion *quat1, const EPhysics_Quaternion *quat2) +{ + btQuaternion bt_quat1, bt_quat2, bt_quat; + + if ((!quat1) || (!quat2)) + { + ERR("Can't operate over null quaternions."); + return NULL; + } + + bt_quat1 = btQuaternion(quat1->x, quat1->y, quat1->z, quat1->w); + bt_quat2 = btQuaternion(quat2->x, quat2->y, quat2->z, quat2->w); + bt_quat = bt_quat1 + bt_quat2; + + return ephysics_quaternion_new(bt_quat.x(), bt_quat.y(), bt_quat.z(), + bt_quat.getW()); +} + +EAPI EPhysics_Quaternion * +ephysics_quaternion_diff(const EPhysics_Quaternion *quat1, const EPhysics_Quaternion *quat2) +{ + btQuaternion bt_quat1, bt_quat2, bt_quat; + + if ((!quat1) || (!quat2)) + { + ERR("Can't operate over null quaternions."); + return NULL; + } + + bt_quat1 = btQuaternion(quat1->x, quat1->y, quat1->z, quat1->w); + bt_quat2 = btQuaternion(quat2->x, quat2->y, quat2->z, quat2->w); + bt_quat = bt_quat1 - bt_quat2; + + return ephysics_quaternion_new(bt_quat.x(), bt_quat.y(), bt_quat.z(), + bt_quat.getW()); +} + +EAPI EPhysics_Quaternion * +ephysics_quaternion_multiply(const EPhysics_Quaternion *quat1, const EPhysics_Quaternion *quat2) +{ + btQuaternion bt_quat1, bt_quat2, bt_quat; + + if ((!quat1) || (!quat2)) + { + ERR("Can't operate over null quaternions."); + return NULL; + } + + bt_quat1 = btQuaternion(quat1->x, quat1->y, quat1->z, quat1->w); + bt_quat2 = btQuaternion(quat2->x, quat2->y, quat2->z, quat2->w); + bt_quat = bt_quat1 * bt_quat2; + + return ephysics_quaternion_new(bt_quat.x(), bt_quat.y(), bt_quat.z(), + bt_quat.getW()); +} + +EAPI EPhysics_Quaternion * +ephysics_quaternion_slerp(const EPhysics_Quaternion *quat1, const EPhysics_Quaternion *quat2, double ratio) +{ + btQuaternion bt_quat1, bt_quat2; + + if ((!quat1) || (!quat2)) + { + ERR("Can't operate over null quaternions."); + return NULL; + } + + bt_quat1 = btQuaternion(quat1->x, quat1->y, quat1->z, quat1->w); + bt_quat2 = btQuaternion(quat2->x, quat2->y, quat2->z, quat2->w); + bt_quat1.slerp(bt_quat2, ratio); + + return ephysics_quaternion_new(bt_quat1.x(), bt_quat1.y(), bt_quat1.z(), + bt_quat1.getW()); +} + +EAPI double +ephysics_quaternion_dot(const EPhysics_Quaternion *quat1, const EPhysics_Quaternion *quat2) +{ + btQuaternion bt_quat1, bt_quat2; + + if ((!quat1) || (!quat2)) + { + ERR("Can't operate over null quaternions."); + return 0; + } + + bt_quat1 = btQuaternion(quat1->x, quat1->y, quat1->z, quat1->w); + bt_quat2 = btQuaternion(quat2->x, quat2->y, quat2->z, quat2->w); + + return bt_quat1.dot(bt_quat2); +} + +EAPI double +ephysics_quaternion_angle_get(const EPhysics_Quaternion *quat1, const EPhysics_Quaternion *quat2) +{ + btQuaternion bt_quat1, bt_quat2; + + if ((!quat1) || (!quat2)) + { + ERR("Can't operate over null quaternions."); + return 0; + } + + bt_quat1 = btQuaternion(quat1->x, quat1->y, quat1->z, quat1->w); + bt_quat2 = btQuaternion(quat2->x, quat2->y, quat2->z, quat2->w); + + return bt_quat1.angle(bt_quat2); +} + +EAPI double +ephysics_quaternion_length_get(const EPhysics_Quaternion *quat) +{ + btQuaternion bt_quat; + + if (!quat) + { + ERR("Can't operate over a null quaternion."); + return 0; + } + + bt_quat = btQuaternion(quat->x, quat->y, quat->z, quat->w); + return bt_quat.length(); +} + +EAPI double +ephysics_quaternion_length2_get(const EPhysics_Quaternion *quat) +{ + btQuaternion bt_quat; + + if (!quat) + { + ERR("Can't operate over a null quaternion."); + return 0; + } + + bt_quat = btQuaternion(quat->x, quat->y, quat->z, quat->w); + return bt_quat.length2(); +} + +#ifdef __cplusplus +} +#endif