22 #ifndef OPENRAVE_GEOMETRY_H
23 #define OPENRAVE_GEOMETRY_H
32 #ifndef RAVE_DEPRECATED
33 #define RAVE_DEPRECATED
36 #define MATH_ASSERT BOOST_ASSERT
39 #define MATH_ASSERT assert
50 #ifdef OPENRAVE_MATH_SQRT_FLOAT
59 #ifdef OPENRAVE_MATH_SQRT_DOUBLE
61 return OPENRAVE_MATH_SQRT_DOUBLE(f);
69 #ifdef OPENRAVE_MATH_SIN_FLOAT
79 #ifdef OPENRAVE_MATH_SIN_DOUBLE
81 return OPENRAVE_MATH_SIN_DOUBLE(f);
89 #ifdef OPENRAVE_MATH_COS_FLOAT
99 #ifdef OPENRAVE_MATH_COS_DOUBLE
101 return OPENRAVE_MATH_COS_DOUBLE(f);
109 #ifdef OPENRAVE_MATH_FABS_FLOAT
119 #ifdef OPENRAVE_MATH_FABS_DOUBLE
121 return OPENRAVE_MATH_FABS_DOUBLE(f);
129 #ifdef OPENRAVE_MATH_ACOS_FLOAT
139 #ifdef OPENRAVE_MATH_ACOS_DOUBLE
141 return OPENRAVE_MATH_ACOS_DOUBLE(f);
149 #ifdef OPENRAVE_MATH_ASIN_FLOAT
159 #ifdef OPENRAVE_MATH_ASIN_DOUBLE
161 return OPENRAVE_MATH_ASIN_DOUBLE(f);
169 #ifdef OPENRAVE_MATH_ATAN2_FLOAT
175 return atan2f(fy,fx);
179 #ifdef OPENRAVE_MATH_ATAN2_DOUBLE
180 inline double MATH_ATAN2(
double fy,
double fx) {
181 return OPENRAVE_MATH_ATAN2_DOUBLE(fy,fx);
194 template <
typename T>
212 MATH_ASSERT(pf != NULL);
x = (T)pf[0];
y = (T)pf[1];
z = (T)pf[2];
w = 0;
222 template <
typename U>
224 x = (T)r.
x;
y = (T)r.
y;
z = (T)r.
z;
w = (T)r.
w;
return *
this;
229 return x*v.
x +
y*v.
y +
z*v.
z +
w*v.
w;
232 return x*v.
x +
y*v.
y +
z*v.
z;
239 if(( f < T(1)-std::numeric_limits<T>::epsilon()) ||( f > T(1)+std::numeric_limits<T>::epsilon()) ) {
243 x /= f;
y /= f;
z /= f; w /= f;
249 if(( f < T(1)-std::numeric_limits<T>::epsilon()) ||( f > T(1)+std::numeric_limits<T>::epsilon()) ) {
252 x /= f;
y /= f; z /= f;
267 inline void Set3(
const T* pvals) {
268 x = pvals[0];
y = pvals[1];
z = pvals[2];
270 inline void Set3(T val1, T val2, T val3) {
271 x = val1;
y = val2;
z = val3;
273 inline void Set4(
const T* pvals) {
274 x = pvals[0];
y = pvals[1];
z = pvals[2];
w = pvals[3];
276 inline void Set4(T val1, T val2, T val3, T val4) {
277 x = val1;
y = val2;
z = val3;
w = val4;
282 ucrossv[0] =
y * v[2] -
z * v[1];
283 ucrossv[1] =
z * v[0] -
x * v[2];
284 ucrossv[2] =
x * v[1] -
y * v[0];
289 Cross(*
this, v);
return *
this;
294 ucrossv[0] = u[1] * v[2] - u[2] * v[1];
295 ucrossv[1] = u[2] * v[0] - u[0] * v[2];
296 ucrossv[2] = u[0] * v[1] - u[1] * v[0];
318 x += T(r.
x);
y += T(r.
y);
z += T(r.
z);
w += T(r.
w);
return *
this;
321 x -= T(r.
x);
y -= T(r.
y);
z -= T(r.
z);
w -= T(r.
w);
return *
this;
324 x *= T(r.
x);
y *= T(r.
y);
z *= T(r.
z);
w *= T(r.
w);
return *
this;
328 x *= k;
y *= k;
z *= k;
w *= k;
return *
this;
331 T k=1/_k;
x *= k;
y *= k;
z *= k;
w *= k;
return *
this;
337 template <
typename U>
friend std::ostream& operator<<(std::ostream& O, const RaveVector<U>&v);
343 ucrossv[0] =
y * v[2] -
z * v[1];
344 ucrossv[1] =
z * v[0] -
x * v[2];
345 ucrossv[2] =
x * v[1] -
y * v[0];
350 template <
typename T>
361 template <
typename T>
376 template <
typename T>
386 T fnorm =
rot.lengthsqr4();
418 v.
x = (1-yy-zz) * r.
x + (xy-zw) * r.
y + (xz+yw)*r.
z;
419 v.
y = (xy+zw) * r.
x + (1-xx-zz) * r.
y + (yz-xw)*r.
z;
420 v.
z = (xz-yw) * r.
x + (yz+xw) * r.
y + (1-xx-yy)*r.
z;
433 T fnorm = t.
rot.lengthsqr4();
448 T fnorm = t.
rot.lengthsqr4();
473 T fnorm =
rot.lengthsqr4();
478 template <
typename U>
friend std::ostream& operator<<(std::ostream& O, const RaveTransform<U>&v);
488 template <
typename T>
497 m[0] = T(t.
m[0]);
m[1] = T(t.
m[1]);
m[2] = T(t.
m[2]);
m[3] = T(t.
m[3]);
498 m[4] = T(t.
m[4]);
m[5] = T(t.
m[5]);
m[6] = T(t.
m[6]);
m[7] = T(t.
m[7]);
499 m[8] = T(t.
m[8]);
m[9] = T(t.
m[9]);
m[10] = T(t.
m[10]);
m[11] = T(t.
m[11]);
505 m[0] = 1;
m[1] = 0;
m[2] = 0;
506 m[4] = 0;
m[5] = 1;
m[6] = 0;
507 m[8] = 0;
m[9] = 0;
m[10] = 1;
511 inline void rotfrommat(T m_00, T m_01, T m_02, T m_10, T m_11, T m_12, T m_20, T m_21, T m_22) {
512 m[0] = m_00;
m[1] = m_01;
m[2] = m_02;
m[3] = 0;
513 m[4] = m_10;
m[5] = m_11;
m[6] = m_12;
m[7] = 0;
514 m[8] = m_20;
m[9] = m_21;
m[10] = m_22;
m[11] = 0;
517 inline T
rot(
int i,
int j)
const {
521 inline T&
rot(
int i,
int j) {
526 template <
typename U>
529 v[0] = r[0] *
m[0] + r[1] *
m[1] + r[2] *
m[2] +
trans.x;
530 v[1] = r[0] *
m[4] + r[1] *
m[5] + r[2] *
m[6] +
trans.y;
531 v[2] = r[0] *
m[8] + r[1] *
m[9] + r[2] *
m[10] +
trans.z;
538 t.
m[0*4+0] =
m[0*4+0]*r.
m[0*4+0]+
m[0*4+1]*r.
m[1*4+0]+
m[0*4+2]*r.
m[2*4+0];
539 t.
m[0*4+1] =
m[0*4+0]*r.
m[0*4+1]+
m[0*4+1]*r.
m[1*4+1]+
m[0*4+2]*r.
m[2*4+1];
540 t.
m[0*4+2] =
m[0*4+0]*r.
m[0*4+2]+
m[0*4+1]*r.
m[1*4+2]+
m[0*4+2]*r.
m[2*4+2];
541 t.
m[1*4+0] =
m[1*4+0]*r.
m[0*4+0]+
m[1*4+1]*r.
m[1*4+0]+
m[1*4+2]*r.
m[2*4+0];
542 t.
m[1*4+1] =
m[1*4+0]*r.
m[0*4+1]+
m[1*4+1]*r.
m[1*4+1]+
m[1*4+2]*r.
m[2*4+1];
543 t.
m[1*4+2] =
m[1*4+0]*r.
m[0*4+2]+
m[1*4+1]*r.
m[1*4+2]+
m[1*4+2]*r.
m[2*4+2];
544 t.
m[2*4+0] =
m[2*4+0]*r.
m[0*4+0]+
m[2*4+1]*r.
m[1*4+0]+
m[2*4+2]*r.
m[2*4+0];
545 t.
m[2*4+1] =
m[2*4+0]*r.
m[0*4+1]+
m[2*4+1]*r.
m[1*4+1]+
m[2*4+2]*r.
m[2*4+1];
546 t.
m[2*4+2] =
m[2*4+0]*r.
m[0*4+2]+
m[2*4+1]*r.
m[1*4+2]+
m[2*4+2]*r.
m[2*4+2];
558 template <
typename U>
561 v.
x = r.
x *
m[0] + r.
y *
m[1] + r.
z *
m[2];
562 v.
y = r.
x *
m[4] + r.
y *
m[5] + r.
z *
m[6];
563 v.
z = r.
x *
m[8] + r.
y *
m[9] + r.
z *
m[10];
569 t.
m[0*4+0] =
m[0*4+0]*r.
m[0*4+0]+
m[0*4+1]*r.
m[1*4+0]+
m[0*4+2]*r.
m[2*4+0];
570 t.
m[0*4+1] =
m[0*4+0]*r.
m[0*4+1]+
m[0*4+1]*r.
m[1*4+1]+
m[0*4+2]*r.
m[2*4+1];
571 t.
m[0*4+2] =
m[0*4+0]*r.
m[0*4+2]+
m[0*4+1]*r.
m[1*4+2]+
m[0*4+2]*r.
m[2*4+2];
572 t.
m[1*4+0] =
m[1*4+0]*r.
m[0*4+0]+
m[1*4+1]*r.
m[1*4+0]+
m[1*4+2]*r.
m[2*4+0];
573 t.
m[1*4+1] =
m[1*4+0]*r.
m[0*4+1]+
m[1*4+1]*r.
m[1*4+1]+
m[1*4+2]*r.
m[2*4+1];
574 t.
m[1*4+2] =
m[1*4+0]*r.
m[0*4+2]+
m[1*4+1]*r.
m[1*4+2]+
m[1*4+2]*r.
m[2*4+2];
575 t.
m[2*4+0] =
m[2*4+0]*r.
m[0*4+0]+
m[2*4+1]*r.
m[1*4+0]+
m[2*4+2]*r.
m[2*4+0];
576 t.
m[2*4+1] =
m[2*4+0]*r.
m[0*4+1]+
m[2*4+1]*r.
m[1*4+1]+
m[2*4+2]*r.
m[2*4+1];
577 t.
m[2*4+2] =
m[2*4+0]*r.
m[0*4+2]+
m[2*4+1]*r.
m[1*4+2]+
m[2*4+2]*r.
m[2*4+2];
589 inv.
m[0*4+0] =
m[1*4 + 1] *
m[2*4 + 2] -
m[1*4 + 2] *
m[2*4 + 1];
590 inv.
m[0*4+1] = m[0*4 + 2] * m[2*4 + 1] - m[0*4 + 1] * m[2*4 + 2];
591 inv.
m[0*4+2] = m[0*4 + 1] * m[1*4 + 2] - m[0*4 + 2] * m[1*4 + 1];
592 inv.
m[1*4+0] = m[1*4 + 2] * m[2*4 + 0] - m[1*4 + 0] * m[2*4 + 2];
593 inv.
m[1*4+1] = m[0*4 + 0] * m[2*4 + 2] - m[0*4 + 2] * m[2*4 + 0];
594 inv.
m[1*4+2] = m[0*4 + 2] * m[1*4 + 0] - m[0*4 + 0] * m[1*4 + 2];
595 inv.
m[2*4+0] = m[1*4 + 0] * m[2*4 + 1] - m[1*4 + 1] * m[2*4 + 0];
596 inv.
m[2*4+1] = m[0*4 + 1] * m[2*4 + 0] - m[0*4 + 0] * m[2*4 + 1];
597 inv.
m[2*4+2] = m[0*4 + 0] * m[1*4 + 1] - m[0*4 + 1] * m[1*4 + 0];
598 T fdet = m[0*4 + 2] * inv.
m[2*4+0] + m[1*4 + 2] * inv.
m[2*4+1] + m[2*4 + 2] * inv.
m[2*4+2];
601 inv.
m[0*4+0] *= fdet; inv.
m[0*4+1] *= fdet; inv.
m[0*4+2] *= fdet;
602 inv.
m[1*4+0] *= fdet; inv.
m[1*4+1] *= fdet; inv.
m[1*4+2] *= fdet;
603 inv.
m[2*4+0] *= fdet; inv.
m[2*4+1] *= fdet; inv.
m[2*4+2] *= fdet;
608 template <
typename U>
611 right.
x =
m[0]; up.
x =
m[1]; dir.
x =
m[2];
612 right.
y =
m[4]; up.
y =
m[5]; dir.
y =
m[6];
613 right.
z =
m[8]; up.
z =
m[9]; dir.
z =
m[10];
616 template <
typename U>
friend std::ostream& operator<<(std::ostream& O, const RaveTransformMatrix<U>&v);
627 template <
typename T>
640 template <
typename T>
653 template <
typename T>
663 template <
typename T>
672 template <
typename T>
700 template <
typename T>
711 template <
typename T>
720 template <
typename U>
749 template <
typename U>
750 std::ostream& operator<<(std::ostream& O, const RaveVector<U>&v)
752 return O << v.x <<
" " << v.y <<
" " << v.z <<
" " << v.w <<
" ";
755 template <
typename U>
758 return I >> v.
x >> v.
y >> v.
z >> v.
w;
761 template <
typename U>
762 std::ostream& operator<<(std::ostream& O, const RaveTransform<U>&v)
764 return O << v.rot.x <<
" " << v.rot.y <<
" " << v.rot.z <<
" " << v.rot.w <<
" " << v.trans.x <<
" " << v.trans.y <<
" " << v.trans.z <<
" ";
767 template <
typename U>
773 template <
typename U>
774 std::ostream& operator<<(std::ostream& O, const ray<U>&r)
776 return O << r.pos.x <<
" " << r.pos.y <<
" " << r.pos.z <<
" " << r.dir.x <<
" " << r.dir.y <<
" " << r.dir.z <<
" ";
779 template <
typename U>
787 template <
typename U>
788 std::ostream& operator<<(std::ostream& O, const RaveTransformMatrix<U>&v)
790 return O << v.m[0] <<
" " << v.m[4] <<
" " << v.m[8] <<
" " << v.m[1] <<
" " << v.m[5] <<
" " << v.m[9] <<
" " << v.m[2] <<
" " << v.m[6] <<
" " << v.m[10] <<
" " << v.trans.x <<
" " << v.trans.y <<
" " << v.trans.z <<
" ";
794 template <
typename U>
797 return I >> v.
m[0] >> v.
m[4] >> v.
m[8] >> v.
m[1] >> v.
m[5] >> v.
m[9] >> v.
m[2] >> v.
m[6] >> v.
m[10] >> v.
trans.x >> v.
trans.y >> v.
trans.z;
828 T sang =
MATH_SIN(axislen*T(0.5))/axislen;
839 T tr = rotation.
m[4*0+0] + rotation.
m[4*1+1] + rotation.
m[4*2+2];
842 rot[1] = (rotation.
m[4*2+1] - rotation.
m[4*1+2]);
843 rot[2] = (rotation.
m[4*0+2] - rotation.
m[4*2+0]);
844 rot[3] = (rotation.
m[4*1+0] - rotation.
m[4*0+1]);
848 if (rotation.
m[4*1+1] > rotation.
m[4*0+0]) {
849 if (rotation.
m[4*2+2] > rotation.
m[4*1+1]) {
850 rot[3] = (rotation.
m[4*2+2] - (rotation.
m[4*0+0] + rotation.
m[4*1+1])) + 1;
851 rot[1] = (rotation.
m[4*2+0] + rotation.
m[4*0+2]);
852 rot[2] = (rotation.
m[4*1+2] + rotation.
m[4*2+1]);
853 rot[0] = (rotation.
m[4*1+0] - rotation.
m[4*0+1]);
856 rot[2] = (rotation.
m[4*1+1] - (rotation.
m[4*2+2] + rotation.
m[4*0+0])) + 1;
857 rot[3] = (rotation.
m[4*1+2] + rotation.
m[4*2+1]);
858 rot[1] = (rotation.
m[4*0+1] + rotation.
m[4*1+0]);
859 rot[0] = (rotation.
m[4*0+2] - rotation.
m[4*2+0]);
862 else if (rotation.
m[4*2+2] > rotation.
m[4*0+0]) {
863 rot[3] = (rotation.
m[4*2+2] - (rotation.
m[4*0+0] + rotation.
m[4*1+1])) + 1;
864 rot[1] = (rotation.
m[4*2+0] + rotation.
m[4*0+2]);
865 rot[2] = (rotation.
m[4*1+2] + rotation.
m[4*2+1]);
866 rot[0] = (rotation.
m[4*1+0] - rotation.
m[4*0+1]);
869 rot[1] = (rotation.
m[4*0+0] - (rotation.
m[4*1+1] + rotation.
m[4*2+2])) + 1;
870 rot[2] = (rotation.
m[4*0+1] + rotation.
m[4*1+0]);
871 rot[3] = (rotation.
m[4*2+0] + rotation.
m[4*0+2]);
872 rot[0] = (rotation.
m[4*2+1] - rotation.
m[4*1+2]);
888 T ilength2 = 2/length2;
890 T qq1 = ilength2*quat[1]*quat[1];
891 T qq2 = ilength2*quat[2]*quat[2];
892 T qq3 = ilength2*quat[3]*quat[3];
893 t.
m[4*0+0] = 1 - qq2 - qq3;
894 t.
m[4*0+1] = ilength2*(quat[1]*quat[2] - quat[0]*quat[3]);
895 t.
m[4*0+2] = ilength2*(quat[1]*quat[3] + quat[0]*quat[2]);
897 t.
m[4*1+0] = ilength2*(quat[1]*quat[2] + quat[0]*quat[3]);
898 t.
m[4*1+1] = 1 - qq1 - qq3;
899 t.
m[4*1+2] = ilength2*(quat[2]*quat[3] - quat[0]*quat[1]);
901 t.
m[4*2+0] = ilength2*(quat[1]*quat[3] - quat[0]*quat[2]);
902 t.
m[4*2+1] = ilength2*(quat[2]*quat[3] + quat[0]*quat[1]);
903 t.
m[4*2+2] = 1 - qq1 - qq2;
918 T ilength2 = 2/length2;
919 T qq1 = ilength2*quat[1]*quat[1];
920 T qq2 = ilength2*quat[2]*quat[2];
921 T qq3 = ilength2*quat[3]*quat[3];
922 rotation.
m[4*0+0] = 1 - qq2 - qq3;
923 rotation.
m[4*0+1] = ilength2*(quat[1]*quat[2] - quat[0]*quat[3]);
924 rotation.
m[4*0+2] = ilength2*(quat[1]*quat[3] + quat[0]*quat[2]);
925 rotation.
m[4*0+3] = 0;
926 rotation.
m[4*1+0] = ilength2*(quat[1]*quat[2] + quat[0]*quat[3]);
927 rotation.
m[4*1+1] = 1 - qq1 - qq3;
928 rotation.
m[4*1+2] = ilength2*(quat[2]*quat[3] - quat[0]*quat[1]);
929 rotation.
m[4*1+3] = 0;
930 rotation.
m[4*2+0] = ilength2*(quat[1]*quat[3] - quat[0]*quat[2]);
931 rotation.
m[4*2+1] = ilength2*(quat[2]*quat[3] + quat[0]*quat[1]);
932 rotation.
m[4*2+2] = 1 - qq1 - qq2;
933 rotation.
m[4*2+3] = 0;
960 template <
typename T>
964 quat0.
x*quat1.
y + quat0.
y*quat1.
x + quat0.
z*quat1.
w - quat0.
w*quat1.
z,
965 quat0.
x*quat1.
z + quat0.
z*quat1.
x + quat0.
w*quat1.
y - quat0.
y*quat1.
w,
966 quat0.
x*quat1.
w + quat0.
w*quat1.
x + quat0.
y*quat1.
z - quat0.
z*quat1.
y);
975 template <
typename T>
987 template <
typename T>
992 if( quat0.
dot(quat1) < 0 ) {
999 T cosHalfTheta = quat0.
w * qb.
w + quat0.
x * qb.
x + quat0.
y * qb.
y + quat0.
z * qb.
z;
1002 qm.
w = quat0.
w; qm.
x = quat0.
x; qm.
y = quat0.
y; qm.
z = quat0.
z;
1007 T sinHalfTheta =
MATH_SQRT(1 - cosHalfTheta*cosHalfTheta);
1011 qm.
w = (quat0.
w * 0.5f + qb.
w * 0.5f);
1012 qm.
x = (quat0.
x * 0.5f + qb.
x * 0.5f);
1013 qm.
y = (quat0.
y * 0.5f + qb.
y * 0.5f);
1014 qm.
z = (quat0.
z * 0.5f + qb.
z * 0.5f);
1017 T ratioA =
MATH_SIN((1 - t) * halfTheta) / sinHalfTheta;
1018 T ratioB =
MATH_SIN(t * halfTheta) / sinHalfTheta;
1020 qm.
w = (quat0.
w * ratioA + qb.
w * ratioB);
1021 qm.
x = (quat0.
x * ratioA + qb.
x * ratioB);
1022 qm.
y = (quat0.
y * ratioA + qb.
y * ratioB);
1023 qm.
z = (quat0.
z * ratioA + qb.
z * ratioB);
1027 template <
typename T>
1030 return quatSlerp<T>(qa,_qb,t);
1037 template <
typename T>
1040 T xx = 2 * q.
y * q.
y;
1041 T xy = 2 * q.
y * q.
z;
1042 T xz = 2 * q.
y * q.
w;
1043 T xw = 2 * q.
y * q.
x;
1044 T yy = 2 * q.
z * q.
z;
1045 T yz = 2 * q.
z * q.
w;
1046 T yw = 2 * q.
z * q.
x;
1047 T zz = 2 * q.
w * q.
w;
1048 T zw = 2 * q.
w * q.
x;
1050 v.
x = (1-yy-zz) * t.
x + (xy-zw) * t.
y + (xz+yw)*t.
z;
1051 v.
y = (xy+zw) * t.
x + (1-xx-zz) * t.
y + (yz-xw)*t.
z;
1052 v.
z = (xz-yw) * t.
x + (yz+xw) * t.
y + (1-xx-yy)*t.
z;
1062 template<
typename T>
1067 T fcos = sourcedir.
dot3(targetdir);
1075 rottodirection -= sourcedir * sourcedir.
dot3(rottodirection);
1078 rottodirection -= sourcedir * sourcedir.
dot3(rottodirection);
1092 template<
typename T>
1096 T angle =
MATH_ATAN2(-quat.
w*axis.
z-quat.
z*axis.
y-quat.
y*axis.
x,quat.
x*axislen);
1099 return std::make_pair(2*angle,
quatMultiply(normalizingquat,quat));
1106 template<
typename T>
1109 T sinang = quat.
y*quat.
y+quat.
z*quat.
z+quat.
w*quat.
w;
1129 template<
typename T>
1135 template <
typename T>
1142 template <
typename T>
1156 template<
typename T>
1171 up -= dir * dir.
dot3(up);
1175 up -= dir * dir.
dot3(up);
1182 t.
m[0] = right.
x; t.
m[1] = up.
x; t.
m[2] = dir.
x;
1183 t.
m[4] = right.
y; t.
m[5] = up.
y; t.
m[6] = dir.
y;
1184 t.
m[8] = right.
z; t.
m[9] = up.
z; t.
m[10] = dir.
z;
1185 t.
trans = vcamerapos;
1191 template <
typename T>
1196 T anglesum=0,costheta;
1197 for (
int i=0; i<4; i++) {
1198 v4.
x = verts[i].
x - v->
x;
1199 v4.
y = verts[i].
y - v->
y;
1200 v4.
z = verts[i].
z - v->
z;
1201 v5.
x = verts[(i+1)%4].x - v->
x;
1202 v5.
y = verts[(i+1)%4].
y - v->
y;
1203 v5.
z = verts[(i+1)%4].z - v->
z;
1206 if (m1*m2 <= std::numeric_limits<T>::epsilon()*std::numeric_limits<T>::epsilon()) {
1214 T diff = anglesum - (T)2.0 * M_PI;
1215 return RaveFabs(diff) <= std::numeric_limits<T>::epsilon();
1220 template <
typename T>
1227 for (
int i=0; i<3; i++) {
1228 v4.
x = tri[i].x - v->
x;
1229 v4.
y = tri[i].y - v->
y;
1230 v4.
z = tri[i].z - v->
z;
1231 v5.
x = tri[(i+1)%3].x - v->
x;
1232 v5.
y = tri[(i+1)%3].y - v->
y;
1233 v5.
z = tri[(i+1)%3].z - v->
z;
1236 if (m1*m2 <= std::numeric_limits<T>::epsilon()*std::numeric_limits<T>::epsilon()) {
1242 anglesum += acos(costheta);
1244 T diff = anglesum - (T)2.0 * M_PI;
1245 return RaveFabs(diff) <= std::numeric_limits<T>::epsilon();
1250 template <
typename T>
1260 vd = r.
dir.cross(vpos);
1272 template <
typename T>
1283 vdir.
y = r.
dir.dot3(o.
up);
1467 template <
typename T>
1494 if( (vNorm.
x < fr.
fnear + vNorm.
y) || (vNorm.
x > fr.
ffar - vNorm.
y) ) {
1539 template <
typename T,
typename U>
1542 for(
size_t i = 0; i < vplanes.size(); ++i) {
1616 template <
typename T>
1629 unorm = u31.
cross(u12);
1630 unorm.
w = -unorm.
dot3(u1);
1631 vnorm = v31.
cross(v12);
1632 vnorm.
w = -vnorm.
dot3(v1);
1633 if( vnorm.
dot3(u1) + vnorm.
w > 0 ) {
1636 if( vnorm.
dot3(u2) + vnorm.
w > 0 ) {
1639 if( vnorm.
dot3(u3) + vnorm.
w > 0 ) {
1642 if((b == 7)||(b == 0)) {
1672 T t = vnorm.
dot3(*pu)+vnorm.
w;
1673 p1 = *pu - p1 * (t / vnorm.
dot3(p1));
1674 p2 = *pu - p2 * (t / vnorm.
dot3(p2));
1680 for(
int i = 0; i < 3; ++i) {
1684 vcross = vedges[i].
cross(vnorm);
1685 T t1 = q1.
dot3(vcross);
1686 T t2 = q2.
dot3(vcross);
1689 if((t1 >= 0)&&(t2 >= 0)) {
1692 if((t1 > 0)&&(t2 < 0)) {
1695 p1 -= dq*(t1/dq.
dot3(vcross));
1697 else if((t1 < 0)&&(t2 > 0)) {
1700 p2 -= dq*(t2/dq.
dot3(vcross));
1704 contactpos = 0.5f * (p1 + p2);
1713 template <
typename T>
1729 template <
typename T>
1739 template <
typename T>
1755 template <
typename T>
1781 template <
typename T>
2349 template <
typename T>
2359 fDist += vn.
x * vn.
x;
2362 fDist += vn.
y * vn.
y;
2365 fDist += vn.
z * vn.
z;