void RigidCoordinator::coordinate(const ParticleVector& particles, const float proceedTime)
{
const Math::Vector3d& objectCenter = getCenter( particles );
const Math::Vector3d& velocityAverage = getAverageVelosity( particles );
for( ParticleVector::const_iterator iter = particles.begin(); iter != particles.end(); ++iter ) {
(*iter)->setVelocity( velocityAverage );
}
for( ParticleVector::const_iterator iter = particles.begin(); iter != particles.end(); ++iter ) {
(*iter)->addCenter( -1.0 * objectCenter );
}
//std::cout << getCenter( particles ).x << std::endl;
//std::cout << getCenter( particles ).y << std::endl;
//std::cout << getCenter( particles ).z << std::endl;
//assert( getCenter( particles ) == Math::Vector3d( 0.0, 0.0, 0.0 ) );
Math::Vector3d inertiaMoment( 0.0, 0.0, 0.0 );
Math::Vector3d torque( 0.0, 0.0, 0.0 );
for( ParticleVector::const_iterator iter = particles.begin(); iter != particles.end(); ++iter ) {
Particle* particle = (*iter);
const Math::Vector3d& center = particle->getCenter();
Math::Vector3d particleMoment( pow( center.getY(), 2) + pow( center.getZ(), 2),
pow( center.getZ(), 2 ) + pow( center.getX(), 2),
pow( center.getX(), 2 ) + pow( center.getY(), 2) );
inertiaMoment += (particleMoment) * particle->getMass();
const Math::Vector3d diffVector( Math::Vector3d( 0.0, 0.0, 0.0), particle->getCenter() );
const Math::Vector3d& particleTorque = diffVector.getOuterProduct( particle->getForce() * particle->getVolume() );
torque += particleTorque;
}
getAngleVelosity( inertiaMoment , torque, proceedTime );
if( Math::Tolerances::isEqualStrictly( angleVelosity.getLength() ) ) {
for( ParticleVector::const_iterator iter = particles.begin(); iter != particles.end(); ++iter ) {
(*iter)->addCenter( objectCenter );
}
convertToFluidForce( particles);
return;
}
const float rotateAngle = angleVelosity.getLength() * proceedTime;
if( rotateAngle < 1.0e-5 ) {
for( ParticleVector::const_iterator iter = particles.begin(); iter != particles.end(); ++iter ) {
(*iter)->addCenter( objectCenter );
}
convertToFluidForce( particles);
return;
}
Math::Quaternion quaternion( angleVelosity.getNormalized(), rotateAngle );
const Math::Matrix3d& rotateMatrix = quaternion.getMatrix();
/* for( ParticleVector::const_iterator iter = particles.begin(); iter != particles.end(); ++iter ) {
(*iter)->variable.center.rotate( rotateMatrix );
}
for( ParticleVector::const_iterator iter = particles.begin(); iter != particles.end(); ++iter ) {
(*iter)->variable.center += objectCenter;
} */
convertToFluidForce( particles );
}
