diff --git a/Extras/InverseDynamics/IDRandomUtil.cpp b/Extras/InverseDynamics/IDRandomUtil.cpp
index fcf879573..fff352490 100644
--- a/Extras/InverseDynamics/IDRandomUtil.cpp
+++ b/Extras/InverseDynamics/IDRandomUtil.cpp
@@ -63,7 +63,7 @@ vec3 randomAxis() {
         axis(1) = randomFloat(-1.0, 1.0);
         axis(2) = randomFloat(-1.0, 1.0);
 
-        length = std::sqrt(std::pow(axis(0), 2) + std::pow(axis(1), 2) + std::pow(axis(2), 2));
+        length = BT_ID_SQRT(BT_ID_POW(axis(0), 2) + BT_ID_POW(axis(1), 2) + BT_ID_POW(axis(2), 2));
     } while (length < 0.01);
 
     return axis / length;
diff --git a/Extras/InverseDynamics/invdyn_bullet_comparison.cpp b/Extras/InverseDynamics/invdyn_bullet_comparison.cpp
index 21e693f62..a32cf828d 100644
--- a/Extras/InverseDynamics/invdyn_bullet_comparison.cpp
+++ b/Extras/InverseDynamics/invdyn_bullet_comparison.cpp
@@ -172,7 +172,7 @@ int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &g
                 printf("bt:ddot_q[%d]= %f, id:ddot_q= %e, diff= %e\n", i, joint_accel[i],
                        dot_u(i + dot_u_offset), joint_accel[i] - dot_u(i));
             }
-            *acc_error += std::pow(joint_accel[i] - dot_u(i + dot_u_offset), 2);
+            *acc_error += BT_ID_POW(joint_accel[i] - dot_u(i + dot_u_offset), 2);
         }
     } else {
         vec3 base_dot_omega;
@@ -196,14 +196,14 @@ int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &g
                 printf("bt::base_dot_omega(%d)= %e dot_u[%d]= %e, diff= %e\n", i, base_dot_omega(i),
                        i, dot_u[i], base_dot_omega(i) - dot_u[i]);
             }
-            *acc_error += std::pow(base_dot_omega(i) - dot_u(i), 2);
+            *acc_error += BT_ID_POW(base_dot_omega(i) - dot_u(i), 2);
         }
         for (int i = 0; i < 3; i++) {
             if (verbose) {
                 printf("bt::base_ddot_com(%d)= %e dot_u[%d]= %e, diff= %e\n", i, base_ddot_com(i),
                        i, dot_u[i + 3], base_ddot_com(i) - dot_u[i + 3]);
             }
-            *acc_error += std::pow(base_ddot_com(i) - dot_u(i + 3), 2);
+            *acc_error += BT_ID_POW(base_ddot_com(i) - dot_u(i + 3), 2);
         }
 
         for (int i = 0; i < btmb->getNumDofs(); i++) {
@@ -211,7 +211,7 @@ int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &g
                 printf("bt:ddot_q[%d]= %f, id:ddot_q= %e, diff= %e\n", i, joint_accel[i],
                        dot_u(i + 6), joint_accel[i] - dot_u(i + 6));
             }
-            *acc_error += std::pow(joint_accel[i] - dot_u(i + 6), 2);
+            *acc_error += BT_ID_POW(joint_accel[i] - dot_u(i + 6), 2);
         }
     }
     *acc_error = std::sqrt(*acc_error);
@@ -288,12 +288,12 @@ int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &g
                    diff_basis(1, 2), diff_basis(2, 0), diff_basis(2, 1), diff_basis(2, 2));
         }
         double total_pos_err =
-            std::sqrt(std::pow(diff_com(0), 2) + std::pow(diff_com(1), 2) +
-                      std::pow(diff_com(2), 2) + std::pow(diff_basis(0, 0), 2) +
-                      std::pow(diff_basis(0, 1), 2) + std::pow(diff_basis(0, 2), 2) +
-                      std::pow(diff_basis(1, 0), 2) + std::pow(diff_basis(1, 1), 2) +
-                      std::pow(diff_basis(1, 2), 2) + std::pow(diff_basis(2, 0), 2) +
-                      std::pow(diff_basis(2, 1), 2) + std::pow(diff_basis(2, 2), 2));
+            BT_ID_SQRT(BT_ID_POW(diff_com(0), 2) + BT_ID_POW(diff_com(1), 2) +
+                       BT_ID_POW(diff_com(2), 2) + BT_ID_POW(diff_basis(0, 0), 2) +
+                       BT_ID_POW(diff_basis(0, 1), 2) + BT_ID_POW(diff_basis(0, 2), 2) +
+                       BT_ID_POW(diff_basis(1, 0), 2) + BT_ID_POW(diff_basis(1, 1), 2) +
+                       BT_ID_POW(diff_basis(1, 2), 2) + BT_ID_POW(diff_basis(2, 0), 2) +
+                       BT_ID_POW(diff_basis(2, 1), 2) + BT_ID_POW(diff_basis(2, 2), 2));
         if (verbose) {
             printf("======kin-pos-err: %e\n", total_pos_err);
         }
diff --git a/src/BulletInverseDynamics/IDConfig.hpp b/src/BulletInverseDynamics/IDConfig.hpp
index c34f98941..08bbdcb70 100644
--- a/src/BulletInverseDynamics/IDConfig.hpp
+++ b/src/BulletInverseDynamics/IDConfig.hpp
@@ -14,12 +14,22 @@
 #ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H
 #include <cmath>
 #define BT_ID_WO_BULLET
-#define BT_ID_POW(a,b) std::pow(a,b)
+#define BT_ID_SQRT(x) std::sqrt(x)
+#define BT_ID_FABS(x) std::fabs(x)
+#define BT_ID_COS(x) std::cos(x)
+#define BT_ID_SIN(x) std::sin(x)
+#define BT_ID_ATAN2(x, y) std::atan2(x, y)
+#define BT_ID_POW(x, y) std::pow(x, y)
 #define BT_ID_SNPRINTF snprintf
 #define BT_ID_PI M_PI
 #define BT_ID_USE_DOUBLE_PRECISION
 #else
-#define BT_ID_POW(a,b) btPow(a,b)
+#define BT_ID_SQRT(x) btSqrt(x)
+#define BT_ID_FABS(x) btFabs(x)
+#define BT_ID_COS(x) btCos(x)
+#define BT_ID_SIN(x) btSin(x)
+#define BT_ID_ATAN2(x, y) btAtan2(x, y)
+#define BT_ID_POW(x, y) btPow(x, y)
 #define BT_ID_PI SIMD_PI
 #ifdef _WIN32
 	#define BT_ID_SNPRINTF _snprintf
diff --git a/src/BulletInverseDynamics/IDMath.cpp b/src/BulletInverseDynamics/IDMath.cpp
index 03452ca0c..021217a17 100644
--- a/src/BulletInverseDynamics/IDMath.cpp
+++ b/src/BulletInverseDynamics/IDMath.cpp
@@ -36,7 +36,7 @@ void setZero(mat33 &m) {
 idScalar maxAbs(const vecx &v) {
 	idScalar result = 0.0;
 	for (int i = 0; i < v.size(); i++) {
-		const idScalar tmp = std::fabs(v(i));
+		const idScalar tmp = BT_ID_FABS(v(i));
 		if (tmp > result) {
 			result = tmp;
 		}
@@ -47,7 +47,7 @@ idScalar maxAbs(const vecx &v) {
 idScalar maxAbs(const vec3 &v) {
 	idScalar result = 0.0;
 	for (int i = 0; i < 3; i++) {
-		const idScalar tmp = std::fabs(v(i));
+		const idScalar tmp = BT_ID_FABS(v(i));
 		if (tmp > result) {
 			result = tmp;
 		}
@@ -111,8 +111,8 @@ void sub(const mat3x &a, const mat3x &b, mat3x *result) {
 
 mat33 transformX(const idScalar &alpha) {
 	mat33 T;
-	const idScalar cos_alpha = std::cos(alpha);
-	const idScalar sin_alpha = std::sin(alpha);
+	const idScalar cos_alpha = BT_ID_COS(alpha);
+	const idScalar sin_alpha = BT_ID_SIN(alpha);
 	// [1  0 0]
 	// [0  c s]
 	// [0 -s c]
@@ -133,8 +133,8 @@ mat33 transformX(const idScalar &alpha) {
 
 mat33 transformY(const idScalar &beta) {
 	mat33 T;
-	const idScalar cos_beta = std::cos(beta);
-	const idScalar sin_beta = std::sin(beta);
+	const idScalar cos_beta = BT_ID_COS(beta);
+	const idScalar sin_beta = BT_ID_SIN(beta);
 	// [c 0 -s]
 	// [0 1  0]
 	// [s 0  c]
@@ -155,8 +155,8 @@ mat33 transformY(const idScalar &beta) {
 
 mat33 transformZ(const idScalar &gamma) {
 	mat33 T;
-	const idScalar cos_gamma = std::cos(gamma);
-	const idScalar sin_gamma = std::sin(gamma);
+	const idScalar cos_gamma = BT_ID_COS(gamma);
+	const idScalar sin_gamma = BT_ID_SIN(gamma);
 	// [ c s 0]
 	// [-s c 0]
 	// [ 0 0 1]
@@ -190,10 +190,10 @@ mat33 tildeOperator(const vec3 &v) {
 }
 
 void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3 *r, mat33 *T) {
-	const idScalar sa = std::sin(alpha);
-	const idScalar ca = std::cos(alpha);
-	const idScalar st = std::sin(theta);
-	const idScalar ct = std::cos(theta);
+	const idScalar sa = BT_ID_SIN(alpha);
+	const idScalar ca = BT_ID_COS(alpha);
+	const idScalar st = BT_ID_SIN(theta);
+	const idScalar ct = BT_ID_COS(theta);
 
 	(*r)(0) = a;
 	(*r)(1) = -sa * d;
@@ -213,8 +213,8 @@ void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec
 }
 
 void bodyTParentFromAxisAngle(const vec3 &axis, const idScalar &angle, mat33 *T) {
-	const idScalar c = cos(angle);
-	const idScalar s = -sin(angle);
+	const idScalar c = BT_ID_COS(angle);
+	const idScalar s = -BT_ID_SIN(angle);
 	const idScalar one_m_c = 1.0 - c;
 
 	const idScalar &x = axis(0);
@@ -347,19 +347,19 @@ bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
 		}
 	}
 	// check symmetry
-	if (std::fabs(I(1, 0) - I(0, 1)) > kIsZero) {
+	if (BT_ID_FABS(I(1, 0) - I(0, 1)) > kIsZero) {
 		error_message("invalid inertia tensor for body %d I(1,0)!=I(0,1). I(1,0)-I(0,1)= "
 					  "%e\n",
 					  index, I(1, 0) - I(0, 1));
 		return false;
 	}
-	if (std::fabs(I(2, 0) - I(0, 2)) > kIsZero) {
+	if (BT_ID_FABS(I(2, 0) - I(0, 2)) > kIsZero) {
 		error_message("invalid inertia tensor for body %d I(2,0)!=I(0,2). I(2,0)-I(0,2)= "
 					  "%e\n",
 					  index, I(2, 0) - I(0, 2));
 		return false;
 	}
-	if (std::fabs(I(1, 2) - I(2, 1)) > kIsZero) {
+	if (BT_ID_FABS(I(1, 2) - I(2, 1)) > kIsZero) {
 		error_message("invalid inertia tensor body %d I(1,2)!=I(2,1). I(1,2)-I(2,1)= %e\n", index,
 					  I(1, 2) - I(2, 1));
 		return false;
@@ -375,7 +375,7 @@ bool isValidTransformMatrix(const mat33 &m) {
 	// check for unit length column vectors
 	for (int i = 0; i < 3; i++) {
 		const idScalar length_minus_1 =
-			std::fabs(m(0, i) * m(0, i) + m(1, i) * m(1, i) + m(2, i) * m(2, i) - 1.0);
+			BT_ID_FABS(m(0, i) * m(0, i) + m(1, i) * m(1, i) + m(2, i) * m(2, i) - 1.0);
 		if (length_minus_1 > kAxisLengthEpsilon) {
 			error_message("Not a valid rotation matrix (column %d not unit length)\n"
 						  "column = [%.18e %.18e %.18e]\n"
@@ -386,17 +386,17 @@ bool isValidTransformMatrix(const mat33 &m) {
 		}
 	}
 	// check for orthogonal column vectors
-	if (std::fabs(m(0, 0) * m(0, 1) + m(1, 0) * m(1, 1) + m(2, 0) * m(2, 1)) > kAxisLengthEpsilon) {
+	if (BT_ID_FABS(m(0, 0) * m(0, 1) + m(1, 0) * m(1, 1) + m(2, 0) * m(2, 1)) > kAxisLengthEpsilon) {
 		error_message("Not a valid rotation matrix (columns 0 and 1 not orthogonal)\n");
 		print_mat(m);
 		return false;
 	}
-	if (std::fabs(m(0, 0) * m(0, 2) + m(1, 0) * m(1, 2) + m(2, 0) * m(2, 2)) > kAxisLengthEpsilon) {
+	if (BT_ID_FABS(m(0, 0) * m(0, 2) + m(1, 0) * m(1, 2) + m(2, 0) * m(2, 2)) > kAxisLengthEpsilon) {
 		error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
 		print_mat(m);
 		return false;
 	}
-	if (std::fabs(m(0, 1) * m(0, 2) + m(1, 1) * m(1, 2) + m(2, 1) * m(2, 2)) > kAxisLengthEpsilon) {
+	if (BT_ID_FABS(m(0, 1) * m(0, 2) + m(1, 1) * m(1, 2) + m(2, 1) * m(2, 2)) > kAxisLengthEpsilon) {
 		error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
 		print_mat(m);
 		return false;
@@ -411,15 +411,15 @@ bool isValidTransformMatrix(const mat33 &m) {
 }
 
 bool isUnitVector(const vec3 &vector) {
-	return std::fabs(vector(0) * vector(0) + vector(1) * vector(1) + vector(2) * vector(2) - 1.0) <
+	return BT_ID_FABS(vector(0) * vector(0) + vector(1) * vector(1) + vector(2) * vector(2) - 1.0) <
 		   kIsZero;
 }
 
 vec3 rpyFromMatrix(const mat33 &rot) {
 	vec3 rpy;
-	rpy(2) = std::atan2(-rot(1, 0), rot(0, 0));
-	rpy(1) = std::atan2(rot(2, 0), std::cos(rpy(2)) * rot(0, 0) - std::sin(rpy(0)) * rot(1, 0));
-	rpy(0) = std::atan2(-rot(2, 0), rot(2, 2));
+	rpy(2) = BT_ID_ATAN2(-rot(1, 0), rot(0, 0));
+	rpy(1) = BT_ID_ATAN2(rot(2, 0), BT_ID_COS(rpy(2)) * rot(0, 0) - BT_ID_SIN(rpy(0)) * rot(1, 0));
+	rpy(0) = BT_ID_ATAN2(-rot(2, 0), rot(2, 2));
 	return rpy;
 }
 }
diff --git a/src/BulletInverseDynamics/MultiBodyTree.cpp b/src/BulletInverseDynamics/MultiBodyTree.cpp
index 4235f138d..c67588d49 100644
--- a/src/BulletInverseDynamics/MultiBodyTree.cpp
+++ b/src/BulletInverseDynamics/MultiBodyTree.cpp
@@ -226,10 +226,10 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ
 				warning_message(
 					"axis of motion not a unit axis ([%f %f %f]), will use normalized vector\n",
 					body_axis_of_motion(0), body_axis_of_motion(1), body_axis_of_motion(2));
-				idScalar length = std::sqrt(std::pow(body_axis_of_motion(0), 2) +
-											std::pow(body_axis_of_motion(1), 2) +
-											std::pow(body_axis_of_motion(2), 2));
-				if (length < std::sqrt(std::numeric_limits<idScalar>::min())) {
+				idScalar length = BT_ID_SQRT(BT_ID_POW(body_axis_of_motion(0), 2) +
+									   BT_ID_POW(body_axis_of_motion(1), 2) +
+									   BT_ID_POW(body_axis_of_motion(2), 2));
+				if (length < BT_ID_SQRT(std::numeric_limits<idScalar>::min())) {
 					error_message("axis of motion vector too short (%e)\n", length);
 					return -1;
 				}
diff --git a/test/InverseDynamics/test_invdyn_kinematics.cpp b/test/InverseDynamics/test_invdyn_kinematics.cpp
index fd7772bd1..697691360 100644
--- a/test/InverseDynamics/test_invdyn_kinematics.cpp
+++ b/test/InverseDynamics/test_invdyn_kinematics.cpp
@@ -24,7 +24,7 @@ idScalar calculateNorm(T&);
 // only implemented for vec3
 template <>
 idScalar calculateNorm(vec3& v) {
-    return std::sqrt(BT_ID_POW(v(0), 2) + BT_ID_POW(v(1), 2) + BT_ID_POW(v(2), 2));
+    return BT_ID_SQRT(BT_ID_POW(v(0), 2) + BT_ID_POW(v(1), 2) + BT_ID_POW(v(2), 2));
 }
 
 // template function to convert a DiffType (finite differences)