Vec3D<> CVX_Voxel::force()
{
//forces from internal bonds
Vec3D<> totalForce(0,0,0);
for (int i=0; i<6; i++){
if (links[i]) totalForce += links[i]->force(isNegative((linkDirection)i)); //total force in LCS
}
totalForce = orient.RotateVec3D(totalForce);
//other forces
totalForce += extForce; //external forces
totalForce -= velocity()*mat->globalDampingTranslateC(); //global damping
if (isGravityEnabled()) totalForce.z -= mat->_mass*9.80665; //gravity, according to f=mg
if (isFloorEnabled()) addFloorForces(&totalForce); //adds forces from interacting with the floor
//Collisions: todo
return totalForce;
}
void Structure0815:: iterate
(
double dt )
{
DynVector zero(_dim, 0.0);
DynVector force(3, 0.0);
DynVector totalForce(_gravity);
totalForce *= _totalMass; // Makes gravity acceleration a force
DynVector r(3, 0.0); // Distance vector from center of gravity
DynVector totalTorque(3, 0.0); // Always init with three components
DynVector torque(3, 0.0);
int verticesSize = _vertices.size() / _dim;
for (int iVertex=0; iVertex < verticesSize; iVertex++){
for (int i=0; i < _dim; i++){
force[i] = _forces[iVertex*_dim + i];
totalForce[i] += force[i];
}
for (int i=0; i<_dim; i++){
r[i] = _vertices[iVertex*_dim + i] - _centerOfGravity[i];
}
tarch::la::cross(r, force, torque);
totalTorque += torque;
}
//STRUCTURE_DEBUG("Total mass = " << _totalMass);
STRUCTURE_DEBUG("Total force = " << totalForce);
//STRUCTURE_DEBUG("Center of gravity = " << _centerOfGravity);
STRUCTURE_DEBUG("Total torque = " << totalTorque);
DynVector translVelocityDelta(zero);
if (not _fixed){
// Compute values of next timestep
translVelocityDelta = (totalForce / _totalMass) * dt;
STRUCTURE_DEBUG("translVelocityDelta = " << translVelocityDelta);
}
for (int i=0; i < _dim; i++){
if (_fixedTranslationDirections[i]){
translVelocityDelta[i] = 0.0;
}
}
// Set values of next timestep
DynVector rotForce(3, 0.0);
DynVector rotVelocityDelta(zero);
DynVector writeVelocity(zero);
DynVector writeDisplacement(zero);
DynVector velocityDelta(zero);
double normR = 0.0;
for (int iVertex=0; iVertex < verticesSize; iVertex++){
int index = iVertex *_dim;
// Compute and write velocity
for (int i=0; i<_dim; i++){
r[i] = _vertices[index + i] - _centerOfGravity[i];
}
normR = tarch::la::norm2(r);
tarch::la::cross(totalTorque, r, rotForce);
//rotForce[0] = torque * -1.0 * r[1]; // TODO
//rotForce[1] = torque * r[0]; // TODO
for (int i=0; i < _dim; i++){
rotVelocityDelta[i] = (rotForce[i] / _totalMass) * dt;
}
velocityDelta = rotVelocityDelta + translVelocityDelta;
for (int i=0; i < _dim; i++){
_velocityDeltas[index+i] = velocityDelta[i];
_velocities[index+i] = _oldVelocities[index+i] + _velocityDeltas[index+i];
_displacementDeltas[index+i] =
(_oldVelocities[index+i] + _velocityDeltas[index+i])*dt;
_displacements[index+i] =
_oldDisplacements[index+i] + _displacementDeltas[index+i];
}
}
STRUCTURE_DEBUG("Computed time = " << _time
<< ", computed timesteps = " << _timesteps);
}
void tgBulletContactSpringCable::calculateAndApplyForce(double dt)
{
#ifndef BT_NO_PROFILE
BT_PROFILE("calculateAndApplyForce");
#endif //BT_NO_PROFILE
const double tension = getTension();
const double currLength = getActualLength();
const double deltaStretch = currLength - m_prevLength;
m_velocity = deltaStretch / dt;
m_damping = m_dampingCoefficient * m_velocity;
if (btFabs(tension) * 1.0 < btFabs(m_damping))
{
m_damping =
(m_damping > 0.0 ? tension * 1.0 : -tension * 1.0);
}
const double magnitude = tension + m_damping;
// Apply forces
std::size_t n = m_anchors.size();
for (std::size_t i = 0; i < n; i++)
{
btVector3 force = btVector3 (0.0, 0.0, 0.0);
if (i == 0)
{
btVector3 direction = m_anchors[i + 1]->getWorldPosition() - m_anchors[i]->getWorldPosition();
force = direction.normalize() * magnitude;
}
// Will likely only be true for the last anchor
else if (m_anchors[i]->sliding == false)
{
btVector3 direction = m_anchors[i]->getWorldPosition() - m_anchors[i - 1]->getWorldPosition();
force = -direction.normalize() * magnitude;
}
else if (i < n - 1)
{
// Already normalized
btVector3 direction = m_anchors[i]->getContactNormal();
// Get normal to string
btVector3 back = m_anchors[i - 1]->getWorldPosition();
btVector3 current = m_anchors[i]->getWorldPosition();
btVector3 forward = m_anchors[i + 1]->getWorldPosition();
btVector3 first = (forward - current);
btVector3 second = (back - current);
btVector3 forceDir = first.normalize() + second.normalize();
// For sliding anchors, just figuring out directions for now
force = magnitude * forceDir;
}
else
{
throw std::runtime_error("tgBulletContactSpringCable: First or last anchor is a sliding constraint!!");
}
m_anchors[i]->force = force;
}
btVector3 totalForce(0.0, 0.0, 0.0);
for (std::size_t i = 0; i < n; i++)
{
btRigidBody* body = m_anchors[i]->attachedBody;
btVector3 contactPoint = m_anchors[i]->getRelativePosition();
body->activate();
totalForce += m_anchors[i]->force;
btVector3 impulse = m_anchors[i]->force* dt;
body->applyImpulse(impulse, contactPoint);
}
if (!totalForce.fuzzyZero())
{
std::cout << "Total Force Error! " << totalForce << std::endl;
throw std::runtime_error("Total force did not sum to zero!");
}
// Finished calculating, so can store things
m_prevLength = currLength;
}
