Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Add and use BT_ID_* math functions for idScalar.

Browse Source
This commit is contained in:
Kibeom Kim
2017-01-26 10:59:14 -08:00
parent a230c941f9
commit 761275df28
6 changed files with 53 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
src/BulletInverseDynamics/IDMath.cpp
View File

@@ -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;
}
}