admin/opensim-libs
1
0
Fork 0
mirror of git://opensimulator.org/git/opensim-libs synced 2026-08-14 00:58:00 +00:00
Code Issues Packages Projects Releases Wiki Activity

update ode test code

Browse Source
This commit is contained in:
UbitUmarov
2022-04-19 10:09:42 +01:00
parent e77ff5c03b
commit fd2209addb
7 changed files with 235 additions and 272 deletions
Download Patch File Download Diff File Expand all files Collapse all files
DotNetOpenMail-0.5.8b/DotNetOpenMail/obj/Release/DesignTimeResolveAssemblyReferencesInput.cache
BIN
View File
Binary file not shown.
DotNetOpenMail-0.5.8b/DotNetOpenMailTests/obj/Release/DesignTimeResolveAssemblyReferencesInput.cache
BIN
View File
Binary file not shown.
trunk/unmanaged/ODE-OpenSim-Test/include/ode/odemath.h
+69 -143
View File
@@ -39,20 +39,36 @@
#define dVALIDMAT3(m) (!(dIsNan(m[0]) || dIsNan(m[1]) || dIsNan(m[2]) || dIsNan(m[3]) || dIsNan(m[4]) || dIsNan(m[5]) || dIsNan(m[6]) || dIsNan(m[7]) || dIsNan(m[8]) || dIsNan(m[9]) || dIsNan(m[10]) || dIsNan(m[11])))
#define dVALIDMAT4(m) (!(dIsNan(m[0]) || dIsNan(m[1]) || dIsNan(m[2]) || dIsNan(m[3]) || dIsNan(m[4]) || dIsNan(m[5]) || dIsNan(m[6]) || dIsNan(m[7]) || dIsNan(m[8]) || dIsNan(m[9]) || dIsNan(m[10]) || dIsNan(m[11]) || dIsNan(m[12]) || dIsNan(m[13]) || dIsNan(m[14]) || dIsNan(m[15]) ))
#if defined(__AVX__)
ODE_PURE_INLINE __m128 load3f(const dReal *a)
{
__m128 ma, mb;
ma = _mm_castpd_ps(_mm_load_sd((double *)a));
mb = _mm_load_ss(a + 2);
return _mm_shuffle_ps(ma, mb, 0x44);
}
ODE_PURE_INLINE void store3f(dReal *res, __m128 ma)
{
__m128 mb;
_mm_store_sd((double *)res, _mm_castps_pd(ma));
mb = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(ma), 0x02));
_mm_store_ss(res + 2, mb);
}
#endif
/* Some vector math */
#if defined(__AVX__)
ODE_PURE_INLINE void dAddVector3(dReal *res, const dReal *a)
{
dReal restmp[4];
__m128 ma, mb;
ma = _mm_loadu_ps(a);
mb = _mm_loadu_ps(res);
ma = load3f(a);
mb = load3f(res);
ma = _mm_add_ps(ma, mb);
_mm_storeu_ps(restmp, ma);
res[0] = restmp[0];
res[1] = restmp[1];
res[2] = restmp[2];
store3f(res, ma);
}
#else
ODE_PURE_INLINE void dAddVector3(dReal *res, const dReal *a)
@@ -86,13 +102,8 @@ ODE_PURE_INLINE void dAddVector3r4(dReal *res, const dReal *a)
#if defined(__AVX__)
ODE_PURE_INLINE void dZeroVector3(dReal *res)
{
dReal restmp[4];
__m128 ma;
ma = _mm_setzero_ps();
_mm_storeu_ps(restmp, ma);
res[0] = restmp[0];
res[1] = restmp[1];
res[2] = restmp[2];
__m128 ma = _mm_setzero_ps();
store3f(res, ma);
}
#else
ODE_PURE_INLINE void dZeroVector3(dReal *res)
@@ -122,16 +133,12 @@ ODE_PURE_INLINE void dZeroVector3r4(dReal *res)
#if defined(__AVX__)
ODE_PURE_INLINE void dAddVectors3(dReal *res, const dReal *a, const dReal *b)
{
dReal restmp[4];
__m128 ma, mb;
ma = _mm_loadu_ps(a);
mb = _mm_loadu_ps(b);
ma = _mm_add_ps(ma, mb);
_mm_storeu_ps(restmp, ma);
res[0] = restmp[0];
res[1] = restmp[1];
res[2] = restmp[2];
store3f(res, ma);
}
#else
ODE_PURE_INLINE void dAddVectors3(dReal *res, const dReal *a, const dReal *b)
@@ -172,16 +179,12 @@ ODE_PURE_INLINE void dAddVectors3r4(dReal *res, const dReal *a, const dReal *b)
#if defined(__AVX__)
ODE_PURE_INLINE void dSubtractVector3(dReal *res, const dReal *a)
{
dReal restmp[4];
__m128 ma, mb;
ma = _mm_loadu_ps(res);
mb = _mm_loadu_ps(a);
ma = _mm_sub_ps(ma, mb);
_mm_storeu_ps(restmp, ma);
res[0] = restmp[0];
res[1] = restmp[1];
res[2] = restmp[2];
store3f(res, ma);
}
#else
ODE_PURE_INLINE void dSubtractVectors3(dReal *res, const dReal *a)
@@ -200,16 +203,12 @@ ODE_PURE_INLINE void dSubtractVectors3(dReal *res, const dReal *a)
#if defined(__AVX__)
ODE_PURE_INLINE void dSubtractVectors3(dReal *res, const dReal *a, const dReal *b)
{
dReal restmp[4];
__m128 ma, mb;
ma = _mm_loadu_ps(a);
mb = _mm_loadu_ps(b);
ma = _mm_sub_ps(ma, mb);
_mm_storeu_ps(restmp, ma);
res[0] = restmp[0];
res[1] = restmp[1];
res[2] = restmp[2];
store3f(res, ma);
}
#else
ODE_PURE_INLINE void dSubtractVectors3(dReal *res, const dReal *a, const dReal *b)
@@ -288,7 +287,6 @@ ODE_PURE_INLINE void dSubNoAliaseVectors3(dReal *res, const dReal *a, const dRea
#if defined(__AVX__)
ODE_PURE_INLINE void dAddScaledVector3(dReal *res, const dReal *a, const dReal scale)
{
dReal restmp[4];
__m128 ma, mb, mc;
ma = _mm_loadu_ps(a);
@@ -298,10 +296,7 @@ ODE_PURE_INLINE void dAddScaledVector3(dReal *res, const dReal *a, const dReal s
ma = _mm_mul_ps(ma, mc);
ma = _mm_add_ps(ma, mb);
_mm_storeu_ps(restmp, ma);
res[0] = restmp[0];
res[1] = restmp[1];
res[2] = restmp[2];
store3f(res, ma);
}
#else
ODE_PURE_INLINE void dAddScaledVector3(dReal *res, const dReal *a, const dReal scale)
@@ -363,7 +358,6 @@ ODE_PURE_INLINE void dAddScaledVector4(dReal *res, const dReal *a, const dReal s
#if defined(__AVX__)
ODE_PURE_INLINE void dAddScaledVectors3(dReal *res, const dReal *a, const dReal *b, dReal a_scale, dReal b_scale)
{
dReal restmp[4];
__m128 ma, mb, mc;
ma = _mm_loadu_ps(a);
@@ -376,10 +370,7 @@ ODE_PURE_INLINE void dAddScaledVectors3(dReal *res, const dReal *a, const dReal
ma = _mm_add_ps(ma, mb);
_mm_storeu_ps(restmp, ma);
res[0] = restmp[0];
res[1] = restmp[1];
res[2] = restmp[2];
store3f(res, ma);
}
#else
ODE_PURE_INLINE void dAddScaledVectors3(dReal *res, const dReal *a, const dReal *b, dReal a_scale, dReal b_scale)
@@ -423,7 +414,6 @@ ODE_PURE_INLINE void dAddScaledVectors3r4(dReal *res, const dReal *a, const dRea
#if defined(__AVX__)
ODE_PURE_INLINE void dAddScaledVector3(dReal *res, const dReal *a, const dReal *b, dReal b_scale)
{
dReal restmp[4];
__m128 ma, mb, mc;
ma = _mm_loadu_ps(a);
@@ -433,10 +423,7 @@ ODE_PURE_INLINE void dAddScaledVector3(dReal *res, const dReal *a, const dReal *
ma = _mm_add_ps(ma, mb);
_mm_storeu_ps(restmp, ma);
res[0] = restmp[0];
res[1] = restmp[1];
res[2] = restmp[2];
store3f(res, ma);
}
#else
ODE_PURE_INLINE void dAddScaledVector3(dReal *res, const dReal *a, const dReal *b, dReal b_scale)
@@ -477,16 +464,12 @@ ODE_PURE_INLINE void dAddScaledVector3r4(dReal *res, const dReal *a, const dReal
#if defined(__AVX__)
ODE_PURE_INLINE void dScaleVector3(dReal *res, dReal nScale)
{
dReal restmp[4];
__m128 ma, mc;
ma = _mm_loadu_ps(res);
mc = _mm_set1_ps(nScale);
ma = _mm_mul_ps(ma, mc);
_mm_storeu_ps(restmp, ma);
res[0] = restmp[0];
res[1] = restmp[1];
res[2] = restmp[2];
store3f(res, ma);
}
#else
ODE_PURE_INLINE void dScaleVector3(dReal *res, dReal nScale)
@@ -563,14 +546,8 @@ ODE_PURE_INLINE void dScaleVector4(dReal *res, dReal nScale)
#if defined(__AVX__)
ODE_PURE_INLINE void dCopyVector3(dReal *res, const dReal *a)
{
dReal restmp[4];
__m128 ma;
ma = _mm_loadu_ps(a);
_mm_storeu_ps(restmp, ma);
res[0] = restmp[0];
res[1] = restmp[1];
res[2] = restmp[2];
__m128 ma = _mm_loadu_ps(a);
store3f(res, ma);
}
#else
ODE_PURE_INLINE void dCopyVector3(dReal *res, const dReal *a)
@@ -605,17 +582,13 @@ ODE_PURE_INLINE void dCopyVector3r4(dReal *res, const dReal *a)
#if defined(__AVX__)
ODE_PURE_INLINE void dFabsVector3(dReal *res)
{
dReal restmp[4];
__m128 ma, sign;
ma = _mm_loadu_ps(res);
sign = _mm_set1_ps(-0.0f);
ma = _mm_andnot_ps(sign, ma);
_mm_storeu_ps(restmp, ma);
res[0] = restmp[0];
res[1] = restmp[1];
res[2] = restmp[2];
store3f(res, ma);
}
#else
ODE_PURE_INLINE void dFabsVector3(dReal *res)
@@ -653,17 +626,13 @@ ODE_PURE_INLINE void dFabsVector3r4(dReal *res)
#if defined(__AVX__)
ODE_PURE_INLINE void dFabsVector3(dReal *res, const dReal *a)
{
dReal restmp[4];
__m128 ma, sign;
ma = _mm_loadu_ps(a);
sign = _mm_set1_ps(-0.0f);
ma = _mm_andnot_ps(sign, ma);
_mm_storeu_ps(restmp, ma);
res[0] = restmp[0];
res[1] = restmp[1];
res[2] = restmp[2];
store3f(res, ma);
}
#else
ODE_PURE_INLINE void dFabsVector3(dReal *res, const dReal *a)
@@ -701,16 +670,12 @@ ODE_PURE_INLINE void dFabsVector3r4(dReal *res, const dReal *a)
#if defined(__AVX__)
ODE_PURE_INLINE void dCopyScaledVector3(dReal *res, const dReal *a, dReal nScale)
{
dReal restmp[4];
__m128 ma, mc;
ma = _mm_loadu_ps(a);
mc = _mm_set1_ps(nScale);
ma = _mm_mul_ps(ma, mc);
_mm_storeu_ps(restmp, ma);
res[0] = restmp[0];
res[1] = restmp[1];
res[2] = restmp[2];
store3f(res, ma);
}
#else
ODE_PURE_INLINE void dCopyScaledVector3(dReal *res, const dReal *a, dReal nScale)
@@ -747,17 +712,13 @@ ODE_PURE_INLINE void dCopyScaledVector3r4(dReal *res, const dReal *a, dReal nSca
#if defined(__AVX__)
ODE_PURE_INLINE void dCopyNegatedVector3(dReal *res, const dReal *a)
{
dReal restmp[4];
__m128 ma, mc;
ma = _mm_loadu_ps(a);
mc = _mm_setzero_ps();
ma = _mm_sub_ps(mc, ma);
_mm_storeu_ps(restmp, ma);
res[0] = restmp[0];
res[1] = restmp[1];
res[2] = restmp[2];
store3f(res, ma);
}
#else
ODE_PURE_INLINE void dCopyNegatedVector3(dReal *res, const dReal *a)
@@ -795,17 +756,13 @@ ODE_PURE_INLINE void dCopyNegatedVector3r4(dReal *res, const dReal *a)
#if defined(__AVX__)
ODE_PURE_INLINE void dNegateVector3(dReal *res)
{
dReal restmp[4];
__m128 ma, mc;
ma = _mm_loadu_ps(res);
mc = _mm_setzero_ps();
ma = _mm_sub_ps(mc, ma);
_mm_storeu_ps(restmp, ma);
res[0] = restmp[0];
res[1] = restmp[1];
res[2] = restmp[2];
store3f(res, ma);
}
#else
ODE_PURE_INLINE void dNegateVector3(dReal *res)
@@ -864,20 +821,11 @@ ODE_PURE_INLINE void dCopyVector4(dReal *res, const dReal *a)
#if defined(__AVX__)
ODE_PURE_INLINE void dSwapVectors3(dReal *a, dReal *b)
{
dReal restmpa[4];
dReal restmpb[4];
__m128 ma,mb;
ma = _mm_loadu_ps(a);
ma = _mm_loadu_ps(b);
_mm_storeu_ps(restmpb, ma);
b[0] = restmpb[0];
b[1] = restmpb[1];
b[2] = restmpb[2];
_mm_storeu_ps(restmpa, mb);
a[0] = restmpa[0];
a[1] = restmpa[1];
a[2] = restmpa[2];
mb = _mm_loadu_ps(b);
store3f(b, ma);
store3f(a, mb);
}
#else
ODE_PURE_INLINE void dSwapVectors3(dReal *a, dReal *b)
@@ -1107,7 +1055,6 @@ ODE_PURE_INLINE void dCalcVectorCross3(dReal *res, const dReal *a, const dReal *
__m128 ma, mb, t1, t2, t3, t4;
ma = _mm_loadu_ps(a);
mb = _mm_loadu_ps(b);
dReal restmp[4];
t1 = _mm_shuffle_ps(ma, ma, _MM_SHUFFLE(3, 0, 2, 1)); // a1 a2 a0 a3
t2 = _mm_shuffle_ps(mb, mb, _MM_SHUFFLE(3, 1, 0, 2)); // b2 b0 b1 b2
@@ -1119,10 +1066,7 @@ ODE_PURE_INLINE void dCalcVectorCross3(dReal *res, const dReal *a, const dReal *
t4 = _mm_mul_ps(t1, t2);
ma = _mm_sub_ps(t3, t4);
_mm_storeu_ps(restmp, ma);
res[0] = restmp[0];
res[1] = restmp[1];
res[2] = restmp[2];
store3f(res, ma);
}
#else
ODE_PURE_INLINE void dCalcVectorCross3(dReal *res, const dReal *a, const dReal *b)
@@ -1269,7 +1213,6 @@ ODE_PURE_INLINE void dAddVectorCross3r4(dReal *res, const dReal *a, const dReal
ODE_PURE_INLINE void dSubtractVectorCross3(dReal *res, const dReal *a, const dReal *b)
{
__m128 ma, mb, mc, t1, t2, t3;
dReal restmp[4];
ma = _mm_loadu_ps(a);
mb = _mm_loadu_ps(b);
@@ -1289,10 +1232,7 @@ ODE_PURE_INLINE void dSubtractVectorCross3(dReal *res, const dReal *a, const dRe
ma = _mm_sub_ps(mc, ma);
_mm_storeu_ps(restmp, ma);
res[0] = restmp[0];
res[1] = restmp[1];
res[2] = restmp[2];
store3f(res, ma);
}
#else
ODE_PURE_INLINE void dSubtractVectorCross3(dReal *res, const dReal *a, const dReal *b)
@@ -1417,7 +1357,6 @@ ODE_PURE_INLINE dReal dCalcPointsDistanceSquare3(const dReal *a, const dReal *b)
ODE_PURE_INLINE void dCalcLerpVectors3(dReal *res, const dReal *a, const dReal *b, const dReal t)
{
__m128 ma, mb, mc, t1, t2;
dReal restmp[4];
ma = _mm_loadu_ps(a);
mb = _mm_loadu_ps(b);
@@ -1427,10 +1366,7 @@ ODE_PURE_INLINE void dCalcLerpVectors3(dReal *res, const dReal *a, const dReal *
t2 = _mm_mul_ps(t1, mc);
mb = _mm_add_ps(ma, t2);
_mm_storeu_ps(restmp, mb);
res[0] = restmp[0];
res[1] = restmp[1];
res[2] = restmp[2];
store3f(res, mb);
}
#else
ODE_PURE_INLINE void dCalcLerpVectors3(dReal *res, const dReal *a, const dReal *b, const dReal t)
@@ -1483,14 +1419,10 @@ ODE_PURE_INLINE void dCalcLerpVectors3r4(dReal *res, const dReal *a, const dReal
ODE_PURE_INLINE void dMultVectors3(dReal *res, const dReal *a, const dReal *b)
{
__m128 ma, mb;
dReal restmp[4];
mb = _mm_loadu_ps(a);
ma = _mm_loadu_ps(b);
ma = _mm_mul_ps(ma, mb);
_mm_storeu_ps(restmp, ma);
res[0] = restmp[0];
res[1] = restmp[1];
res[2] = restmp[2];
store3f(res, ma);
}
#else
ODE_PURE_INLINE void dMultVectors3(dReal *res, const dReal *a, const dReal *b)
@@ -1525,14 +1457,10 @@ ODE_PURE_INLINE void dMultVectors3r4(dReal *res, const dReal *a, const dReal *b)
ODE_PURE_INLINE void dMultVector3(dReal *res, const dReal *a)
{
__m128 ma, mb;
dReal restmp[4];
mb = _mm_loadu_ps(res);
ma = _mm_loadu_ps(a);
ma = _mm_mul_ps(ma, mb);
_mm_storeu_ps(restmp, ma);
res[0] = restmp[0];
res[1] = restmp[1];
res[2] = restmp[2];
store3f(res, ma);
}
#else
ODE_PURE_INLINE void dMultVector3(dReal *res, const dReal *a)
@@ -1568,13 +1496,17 @@ ODE_PURE_INLINE void dMultiply0_331(dReal *res, const dReal *a, const dReal *b)
{
__m128 ma, mb, mc;
dReal restmp[3];
mb = _mm_loadu_ps(b);
ma = _mm_loadu_ps(a);
mc = _mm_dp_ps(ma, mb, 0x71);
restmp[0] = (dReal)_mm_cvtss_f32(mc);
ma = _mm_loadu_ps(a + 4);
mc = _mm_dp_ps(ma, mb, 0x71);
restmp[1] = (dReal)_mm_cvtss_f32(mc);
ma = _mm_loadu_ps(a + 8);
mc = _mm_dp_ps(ma, mb, 0x71);
restmp[2] = (dReal)_mm_cvtss_f32(mc);
@@ -1660,7 +1592,6 @@ it is not equivalent to A*=B.
ODE_PURE_INLINE void dPointRotateTrans(dReal *res, const dReal *r, const dReal *p, const dReal *t)
{
__m128 ma, mb, mr, mt;
dReal restmp[4];
mb = _mm_loadu_ps(p);
ma = _mm_loadu_ps(r);
@@ -1677,10 +1608,7 @@ ODE_PURE_INLINE void dPointRotateTrans(dReal *res, const dReal *r, const dReal *
ma = _mm_loadu_ps(t);
mr = _mm_add_ps(mr, ma);
_mm_storeu_ps(restmp, mr);
res[0] = restmp[0];
res[1] = restmp[1];
res[2] = restmp[2];
store3f(res, mr);
}
#else
ODE_PURE_INLINE void dPointRotateTrans(dReal *res, const dReal *vec, dReal *rot, const dReal *pos)
@@ -1724,7 +1652,6 @@ ODE_PURE_INLINE void dPointRotateTrans_r4(dReal *res, const dReal *vec, dReal *r
ODE_PURE_INLINE void dtriangleRotateTrans(dVector3 *res, dVector3 *invec, const dReal *rot, const dVector3 pos)
{
__m128 mrota, mrotb, mrotc, mpos, ma, mb, mr;
dReal restmp[4];
mrota = _mm_loadu_ps(rot);
mrotb = _mm_loadu_ps(rot + 4);
mrotc = _mm_loadu_ps(rot + 8);
@@ -1741,10 +1668,7 @@ ODE_PURE_INLINE void dtriangleRotateTrans(dVector3 *res, dVector3 *invec, const
mr = _mm_blend_ps(mr, mb, 4);
mr = _mm_add_ps(mr, mpos);
_mm_storeu_ps(restmp, mr);
res[i][0] = restmp[0];
res[i][1] = restmp[1];
res[i][2] = restmp[2];
store3f((dReal *)res, mr);
}
}
#else
@@ -1920,29 +1844,31 @@ ODE_PURE_INLINE dReal dCalcMatrix3Det(const dReal* mat)
*/
ODE_PURE_INLINE dReal dInvertMatrix3(dReal *dst, const dReal *ma)
{
dReal det;
dReal det;
dReal detRecip;
det = dCalcMatrix3Det( ma );
dReal d1 = ma[5] * ma[10] - ma[6] * ma[9];
dReal d2 = ma[6] * ma[8] - ma[4] * ma[10];
dReal d3 = ma[4] * ma[9] - ma[5] * ma[8];
det = ma[0] * d1 + ma[1] * d2 + ma[2] * d3;
if ( det == 0 )
{
return 0;
}
detRecip = dRecip(det);
detRecip = dRecip(det);
dst[0] = ( ma[5]*ma[10] - ma[6]*ma[9] ) * detRecip;
dst[1] = ( ma[9]*ma[2] - ma[1]*ma[10] ) * detRecip;
dst[2] = ( ma[1]*ma[6] - ma[5]*ma[2] ) * detRecip;
dst[0] = d1 * detRecip;
dst[1] = ( ma[9] * ma[2] - ma[1] * ma[10] ) * detRecip;
dst[2] = ( ma[1] * ma[6] - ma[5] * ma[2] ) * detRecip;
dst[4] = ( ma[6]*ma[8] - ma[4]*ma[10] ) * detRecip;
dst[5] = ( ma[0]*ma[10] - ma[8]*ma[2] ) * detRecip;
dst[6] = ( ma[4]*ma[2] - ma[0]*ma[6] ) * detRecip;
dst[4] = d2 * detRecip;
dst[5] = ( ma[0] * ma[10] - ma[8] * ma[2] ) * detRecip;
dst[6] = ( ma[4] * ma[2] - ma[0] * ma[6] ) * detRecip;
dst[8] = ( ma[4]*ma[9] - ma[8]*ma[5] ) * detRecip;
dst[9] = ( ma[8]*ma[1] - ma[0]*ma[9] ) * detRecip;
dst[10] = ( ma[0]*ma[5] - ma[1]*ma[4] ) * detRecip;
dst[8] = d3 * detRecip;
dst[9] = ( ma[8] * ma[1] - ma[0] * ma[9] ) * detRecip;
dst[10] = ( ma[0] * ma[5] - ma[1] * ma[4] ) * detRecip;
return det;
}
trunk/unmanaged/ODE-OpenSim-Test/ode/src/odemath.cpp
+30 -8
View File
@@ -30,14 +30,35 @@
#undef dNormalize3
#undef dNormalize4
// this may be called for vectors `a' with extremely small magnitude, for
// example the result of a cross product on two nearly perpendicular vectors.
// we must be robust to these small vectors. to prevent numerical error,
// first find the component a[i] with the largest magnitude and then scale
// all the components by 1/a[i]. then we can compute the length of `a' and
// scale the components by 1/l. this has been verified to work with vectors
// containing the smallest representable numbers.
/*
#if defined(__AVX__)
int _dSafeNormalize3(dVector3 a)
{
__m128 ma,mb,mc;
mb = _mm_loadu_ps(a);
ma = _mm_dp_ps(mb, mb, 0x71);
dReal l = (dReal)_mm_cvtss_f32(ma);
if (l > dEpsilon)
{
mc = _mm_set_ss(1.0f);
ma = _mm_sqrt_ss(ma);
ma = _mm_div_ss(mc, ma);
ma = _mm_insert_ps(ma,ma,0x0E);
ma = _mm_shuffle_ps(ma,ma, 0x40);
ma = _mm_mul_ps(ma, mb);
_mm_storeu_ps(a, ma);
return 1;
}
a[0] = 1; // if all a's are zero, this is where we'll end up.
a[1] = 0; // return a default unit length vector.
a[2] = 0;
return 0;
}
#else
*/
int _dSafeNormalize3 (dVector3 a)
{
dIASSERT(a);
@@ -54,6 +75,7 @@ int _dSafeNormalize3 (dVector3 a)
a[2] = 0;
return 0;
}
//#endif
/* OLD VERSION */
/*
@@ -90,7 +112,7 @@ int _dSafeNormalize4 (dVector4 a)
{
dAASSERT (a);
dReal l = dCalcVectorLengthSquare4(a);
if (l > 0) {
if (l > dEpsilon) {
l = dRecipSqrt(l);
dScaleVector4(a, l);
return 1;
trunk/unmanaged/ODE-OpenSim-Test/ode/src/quickstep.cpp
+56 -53
View File
@@ -896,62 +896,65 @@ void dxQuickStepIsland_Stage0_Bodies(dxQuickStepperStage0BodiesCallContext *call
dMultiply2_333 (tmp,b->mass.I,b->posr.R);
dMultiply0_333 (I,b->posr.R,tmp);
// compute rotational force
#if 0
// Explicit computation
dMultiply0_331 (tmp,I,b->avel);
dSubtractVectorCross3r4(b->tacc,b->avel,tmp);
#else
// Do the implicit computation based on
//"Stabilizing Gyroscopic Forces in Rigid Multibody Simulations"
// (Lacoursière 2006)
dReal h = callContext->m_stepperCallContext->m_stepSize; // Step size
dVector3 L; // Compute angular momentum
dMultiply0_331(L,I,b->avel);
// Compute a new effective 'inertia tensor'
// for the implicit step: the cross-product
// matrix of the angular momentum plus the
// old tensor scaled by the timestep.
// Itild may not be symmetric pos-definite,
// but we can still use it to compute implicit
// gyroscopic torques.
dMatrix3 Itild={0};
dSetCrossMatrixMinus(Itild,L,4);
for (int ii=0;ii<12;++ii) {
Itild[ii]=Itild[ii]*h+I[ii];
}
// Scale momentum by inverse time to get
// a sort of "torque"
dScaleVector3r4(L,dRecip(h));
// Invert the pseudo-tensor
dMatrix3 itInv;
// This is a closed-form inversion.
// It's probably not numerically stable
// when dealing with small masses with
// a large asymmetry.
// An LU decomposition might be better.
if (dInvertMatrix3(itInv,Itild)!=0)
if(b->invMass > 1e6)
{
// "Divide" the original tensor
// by the pseudo-tensor (on the right)
dMultiply0_333(Itild,I,itInv);
// Subtract an identity matrix
Itild[0]-=1; Itild[5]-=1; Itild[10]-=1;
// Explicit computation
dMultiply0_331 (tmp,I,b->avel);
dSubtractVectorCross3r4(b->tacc,b->avel,tmp);
}
else
{
// Do the implicit computation based on
//"Stabilizing Gyroscopic Forces in Rigid Multibody Simulations"
// (Lacoursière 2006)
dReal h = callContext->m_stepperCallContext->m_stepSize; // Step size
dVector3 L; // Compute angular momentum
dMultiply0_331(L,I,b->avel);
// This new inertia matrix rotates the
// momentum to get a new set of torques
// that will work correctly when applied
// to the old inertia matrix as explicit
// torques with a semi-implicit update
// step.
dVector3 tau0;
dMultiply0_331(tau0,Itild,L);
dAddVector3r4(b->tacc, tau0);
// Add the gyro torques to the torque
// accumulator
// Compute a new effective 'inertia tensor'
// for the implicit step: the cross-product
// matrix of the angular momentum plus the
// old tensor scaled by the timestep.
// Itild may not be symmetric pos-definite,
// but we can still use it to compute implicit
// gyroscopic torques.
dMatrix3 Itild = {0};
dScaleVector3r4(tmp, L, h);
dSetCrossMatrixMinus(Itild , tmp, 4);
for (int ii = 0; ii < 12; ++ii)
Itild[ii] += I[ii];
// Scale momentum by inverse time to get
// a sort of "torque"
dScaleVector3r4(L,dRecip(h));
// Invert the pseudo-tensor
dMatrix3 itInv;
// This is a closed-form inversion.
// It's probably not numerically stable
// when dealing with small masses with
// a large asymmetry.
// An LU decomposition might be better.
if (dInvertMatrix3(itInv,Itild)!=0)
{
// "Divide" the original tensor
// by the pseudo-tensor (on the right)
dMultiply0_333(Itild,I,itInv);
// Subtract an identity matrix
Itild[0]-=1; Itild[5]-=1; Itild[10]-=1;
// This new inertia matrix rotates the
// momentum to get a new set of torques
// that will work correctly when applied
// to the old inertia matrix as explicit
// torques with a semi-implicit update
// step.
dVector3 tau0;
dMultiply0_331(tau0,Itild,L);
dAddVector3r4(b->tacc, tau0);
// Add the gyro torques to the torque
// accumulator
}
}
#endif
}
}
}
trunk/unmanaged/ODE-OpenSim-Test/ode/src/rotation.cpp
+25 -16
View File
@@ -32,7 +32,7 @@ quaternions have the format: (s,vx,vy,vz) where (vx,vy,vz) is the
#include "odemath.h"
#define _R(i,j) R[(i)*4+(j)]
#define _R(i,j) R[4*(i)+(j)]
#define SET_3x3_IDENTITY \
_R(0,0) = REAL(1.0); \
@@ -237,24 +237,33 @@ void dRfromQ (dMatrix3 R, const dQuaternion q)
{
dAASSERT (q && R);
// q = (s,vx,vy,vz)
dReal qq1 = 2 * q[1] * q[1];
dReal qq2 = 2 * q[2] * q[2];
dReal qq3 = 2 * q[3] * q[3];
dReal x2 = q[1] + q[1];
dReal y2 = q[2] + q[2];
dReal z2 = q[3] + q[3];
_R(0,0) = 1 - qq2 - qq3;
_R(0,1) = 2 * (q[1] * q[2] - q[0] * q[3]);
_R(0,2) = 2 * (q[1] * q[3] + q[0] * q[2]);
_R(0,3) = REAL(0.0);
dReal yy2 = q[2] * y2;
dReal zz2 = q[3] * z2;
_R(0,0) = 1.0f - yy2 - zz2;
dReal xy2 = q[1] * y2;
dReal wz2 = q[0] * z2;
_R(0,1) = xy2 - wz2;
dReal xz2 = q[1] * z2;
dReal wy2 = q[0] * y2;
_R(0,2) = xz2 + wy2;
_R(0, 3) = REAL(0.0);
_R(1,0) = 2 * (q[1] * q[2] + q[0] * q[3]);
_R(1,1) = 1 - qq1 - qq3;
_R(1,2) = 2 * (q[2] * q[3] - q[0] * q[1]);
_R(1,3) = REAL(0.0);
_R(1, 0) = xy2 + wz2;
dReal xx2 = q[1] * x2;
_R(1,1) = 1.0f - xx2 - zz2;
dReal wx2 = q[0] * x2;
dReal yz2 = q[2] * z2;
_R(1,2) = yz2 - wx2;
_R(1, 3) = REAL(0.0);
_R(2,0) = 2 * (q[1] * q[3] - q[0] * q[2]);
_R(2,1) = 2 * (q[2] * q[3] + q[0] * q[1]);
_R(2,2) = 1 - qq1 - qq2;
_R(2,3) = REAL(0.0);
_R(2, 0) = xz2 - wy2;
_R(2, 1) = yz2 + wx2;
_R(2,2) = 1.0f - xx2 - yy2;
_R(2, 3) = REAL(0.0);
}
trunk/unmanaged/ODE-OpenSim-Test/ode/src/step.cpp
+55 -52
View File
@@ -430,64 +430,67 @@ void dxStepIsland_Stage0_Bodies(dxStepperStage0BodiesCallContext *callContext)
dMultiply2_333 (tmp,b->mass.I,b->posr.R);
dMultiply0_333 (I,b->posr.R,tmp);
// compute rotational force
#if 0
// Explicit computation
dMultiply0_331 (tmp,I,b->avel);
dSubtractVectorCross3(b->tacc,b->avel,tmp);
#else
// Do the implicit computation based on
//"Stabilizing Gyroscopic Forces in Rigid Multibody Simulations"
// (Lacoursière 2006)
dReal h = callContext->m_stepperCallContext->m_stepSize; // Step size
dVector3 L; // Compute angular momentum
dMultiply0_331(L,I,b->avel);
// Compute a new effective 'inertia tensor'
// for the implicit step: the cross-product
// matrix of the angular momentum plus the
// old tensor scaled by the timestep.
// Itild may not be symmetric pos-definite,
// but we can still use it to compute implicit
// gyroscopic torques.
dMatrix3 Itild={0};
dSetCrossMatrixMinus(Itild,L,4);
for (int ii=0;ii<12;++ii) {
Itild[ii]=Itild[ii]*h+I[ii];
if (b->invMass > 1e6)
{
// Explicit computation
dMultiply0_331 (tmp,I,b->avel);
dSubtractVectorCross3(b->tacc,b->avel,tmp);
}
else
{
// Do the implicit computation based on
//"Stabilizing Gyroscopic Forces in Rigid Multibody Simulations"
// (Lacoursière 2006)
dReal h = callContext->m_stepperCallContext->m_stepSize; // Step size
dVector3 L; // Compute angular momentum
dMultiply0_331(L,I,b->avel);
// Compute a new effective 'inertia tensor'
// for the implicit step: the cross-product
// matrix of the angular momentum plus the
// old tensor scaled by the timestep.
// Itild may not be symmetric pos-definite,
// but we can still use it to compute implicit
// gyroscopic torques.
dMatrix3 Itild={0};
dSetCrossMatrixMinus(Itild,L,4);
for (int ii= 0 ;ii < 12; ++ii) {
Itild[ii] = Itild[ii] * h + I[ii];
}
// Scale momentum by inverse time to get
// a sort of "torque"
dScaleVector3(L,dRecip(h));
// Invert the pseudo-tensor
dMatrix3 itInv;
// This is a closed-form inversion.
// It's probably not numerically stable
// when dealing with small masses with
// a large asymmetry.
// An LU decomposition might be better.
if (dInvertMatrix3(itInv,Itild)!=0) {
// "Divide" the original tensor
// by the pseudo-tensor (on the right)
dMultiply0_333(Itild,I,itInv);
// Subtract an identity matrix
Itild[0]-=1; Itild[5]-=1; Itild[10]-=1;
// Scale momentum by inverse time to get
// a sort of "torque"
dScaleVector3(L,dRecip(h));
// Invert the pseudo-tensor
dMatrix3 itInv;
// This is a closed-form inversion.
// It's probably not numerically stable
// when dealing with small masses with
// a large asymmetry.
// An LU decomposition might be better.
if (dInvertMatrix3(itInv,Itild)!=0) {
// "Divide" the original tensor
// by the pseudo-tensor (on the right)
dMultiply0_333(Itild,I,itInv);
// Subtract an identity matrix
Itild[0]-=1; Itild[5]-=1; Itild[10]-=1;
// This new inertia matrix rotates the
// momentum to get a new set of torques
// that will work correctly when applied
// to the old inertia matrix as explicit
// torques with a semi-implicit update
// step.
dVector3 tau0;
dMultiply0_331(tau0,Itild,L);
// This new inertia matrix rotates the
// momentum to get a new set of torques
// that will work correctly when applied
// to the old inertia matrix as explicit
// torques with a semi-implicit update
// step.
dVector3 tau0;
dMultiply0_331(tau0,Itild,L);
// Add the gyro torques to the torque
// accumulator
for (int ii=0;ii<3;++ii) {
b->tacc[ii]+=tau0[ii];
// Add the gyro torques to the torque
// accumulator
for (int ii=0;ii<3;++ii) {
b->tacc[ii]+=tau0[ii];
}
}
}
#endif
}
bodyIndex = ThrsafeIncrementIntUpToLimit(&callContext->m_inertiaBodyIndex, nb);