tarch::la::Vector<DIMENSIONS,double>
peano::integration::partitioncoupling::builtin::tests::PartitionCoupling4MovingSphereTest::
getExpectedVelocity(
const tarch::la::Vector<DIMENSIONS,double> &spherePosition,
const tarch::la::Vector<DIMENSIONS,double> &velocityPosition,
const tarch::la::Vector<DIMENSIONS,double> &force,
const double &radius, const double &sphereMass,const double &dt
) const {
tarch::la::Vector<DIMENSIONS,double> result(0.0);
tarch::la::Vector<3,double> r(0.0);
tarch::la::Vector<3,double> spherePos(0.0);
tarch::la::Vector<3,double> velocityPos(0.0);
tarch::la::Vector<3,double> force3(0.0);
for (int d = 0; d < DIMENSIONS; d++){
force3(d) = force(d);
spherePos(d) = spherePosition(d);
velocityPos(d) = velocityPosition(d);
}
r = peano::integration::partitioncoupling::builtin::crossProduct(velocityPos-spherePos,force3);
r = peano::integration::partitioncoupling::builtin::crossProduct(r,velocityPos-spherePos);
r = dt/(2.0/5.0*sphereMass*radius*radius)*r;
for (int d = 0; d < DIMENSIONS; d++){
result(d) = r(d);
}
return result;
}
void RigidBody2::AddTorque(const Vec2f& force, const Vec2f& poa) // pointOfApplication
{
Vec3f force3(force.x, force.y, 0);
Vec3f poa3(poa.x - m_pos.x, poa.y - m_pos.y, 0);
Vec3f t = CrossProduct(force3, poa3);
m_torques += t.z;
//std::cout << "Adding torque: x: " << t.x << " y: " << t.y << " z: " << t.z << "\n";
}