void RigidBody3DState::updateMandMinv()
{
assert( unsigned( m_M0.nonZeros() ) == 6 * nbodies() );
assert( m_M0.nonZeros() == m_Minv0.nonZeros() );
assert( unsigned( m_M.nonZeros() ) == 12 * nbodies() );
assert( m_M.nonZeros() == m_Minv.nonZeros() );
for( unsigned bdy_idx = 0; bdy_idx < m_nbodies; ++bdy_idx )
{
// Orientation of the ith body
const Eigen::Map<const Matrix33sr> R{ m_q.segment<9>( 3 * m_nbodies + 9 * bdy_idx ).data() };
assert( fabs( ( R * R.transpose() - Matrix33sr::Identity() ).lpNorm<Eigen::Infinity>() ) <= 1.0e-9 );
assert( fabs( R.determinant() - 1.0 ) <= 1.0e-9 );
// Inertia tensor of the ith body
{
Eigen::Map<Matrix33sc> I{ &m_M.data().value( 3 * m_nbodies + 9 * bdy_idx ) };
const Eigen::Map<const Vector3s> I0{ &m_M0.data().value( 3 * m_nbodies + 3 * bdy_idx ) };
I = R * I0.asDiagonal() * R.transpose();
assert( ( I - I.transpose() ).lpNorm<Eigen::Infinity>() <= 1.0e-12 );
assert( I.determinant() > 0.0 );
}
// Inverse of the inertia tensor of the ith body
{
Eigen::Map<Matrix33sc> Iinv{ &m_Minv.data().value( 3 * m_nbodies + 9 * bdy_idx ) };
const Eigen::Map<const Vector3s> Iinv0{ &m_Minv0.data().value( 3 * m_nbodies + 3 * bdy_idx ) };
Iinv = R * Iinv0.asDiagonal() * R.transpose();
assert( ( Iinv - Iinv.transpose() ).lpNorm<Eigen::Infinity>() <= 1.0e-12 );
assert( Iinv.determinant() > 0.0 );
}
}
assert( MathUtilities::isIdentity( m_M * m_Minv, 1.0e-9 ) );
}
int main(int argc, char *argv[])
{
const cl_uint numBodies = 10;
float *m = cl_malloc_array(float, numBodies);
float *t = cl_malloc_array(float, numBodies);
cl_float4 *a = cl_malloc_array(cl_float4, numBodies);
cl_float4 *v = cl_malloc_array(cl_float4, numBodies);
cl_float4 *p = cl_malloc_array(cl_float4, numBodies);
#ifdef CL_BUILD_RUNTIME
cl_uint type = clGetTypeFromString(CL_USER_DEVICE_TYPE);
cl_uint count = CL_USER_DEVICE_COUNT;
cl_environment_t *pEnv = clCreateEnvironment(KDIR"kernel_nbody.cl",type,count,notify,CL_ARGS);
#else
cl_environment_t *pEnv = clCreateEnvironmentFromBins(&gKernelBins, notify, CL_ARGS);
#endif
if (pEnv)
{
cl_uint i = 0, j = 0;
cl_uint numIterations = (argc > 1?atoi(argv[1]):10);
for (i = 0; i < numBodies; i++)
{
m[i] = frand() * ipow(10,rrand(4,27)); // masses should be 10^4 - 10^27 ("Earth heavy")
frand4(a[i], 1, 3);
frand4(v[i], 1, 2);
frand4(p[i], 4, 8);
t[i] = 0.001f; // 1 millisecond.
}
i = 0;
for (j = 0; j < numIterations; j++)
{
nbodies(pEnv, m, a, v, p, t, numBodies);
#if defined(DARWIN)
printf("[%6u] p={%lf,%lf,%lf} v={%lf,%lf,%lf} a={%lf,%lf,%lf}\n", i,
p[i][0], p[i][1], p[i][2],
v[i][0], v[i][1], v[i][2],
a[i][0], a[i][1], a[i][2]);
#else
printf("[%6u] p={%lf,%lf,%lf} v={%lf,%lf,%lf} a={%lf,%lf,%lf}\n", i,
p[i].s[0], p[i].s[1], p[i].s[2],
v[i].s[0], v[i].s[1], v[i].s[2],
a[i].s[0], a[i].s[1], a[i].s[2]);
#endif
}
clDeleteEnvironment(pEnv);
cl_free(t);
cl_free(m);
cl_free(v);
cl_free(a);
cl_free(p);
}
return 0;
}
const Eigen::Map<const Matrix33sr> RigidBody3DState::getInertia( const unsigned body ) const
{
assert( body < nbodies() );
assert( ( Eigen::Map<const Matrix33sr>( &M().valuePtr()[ 3 * nbodies() + 9 * body ] ) - Eigen::Map<const Matrix33sr>( &M().valuePtr()[ 3 * nbodies() + 9 * body ] ).transpose() ).lpNorm<Eigen::Infinity>() <= 1.0e-12 );
return Eigen::Map<const Matrix33sr>( &M().valuePtr()[ 3 * nbodies() + 9 * body ] );
}
const scalar& RigidBody3DState::getTotalMass( const unsigned body ) const
{
assert( body < nbodies() );
return M().valuePtr()[ 3 * body ];
}
