void Particle::integrate(float dt) {
// An unmovable particle has zero inverseMass.
if (m_inverseMass <= 0.0f) return;
// Verify a non-zero time step.
ASSERT(dt > 0.0f, "Expected a non-zero time step in Particle::Integrate");
// Work out the acceleration from the force.
Vector3 resultingAcceleration(acceleration + m_inverseMass*m_force);
// Update linear velocity from the acceleration.
velocity += dt*resultingAcceleration;
// Impose artificial drag.
velocity *= pow(m_damping, dt);
// Update linear position.
position += dt*velocity;
force = m_force;
// Clear the forces.
clearForce();
}
void Particle::update(float dt) {
acc = totalForce / mass;
vel += acc * dt;
vel *= (drag != 1) ? pow(drag,dt) : 1; // drag!!
pos += vel * dt;
momentum = vel * mass;
clearForce();
}
void RK_CALL manipulator_dynamics_model::computeOutput(double aTime,const ReaK::vect_n<double>& aState, ReaK::vect_n<double>& aOutput) {
setJointStates(aState);
doMotion();
clearForce();
doForce();
aOutput.resize(getOutputsCount());
unsigned int i = 0;
for(std::vector< shared_ptr<system_output> >::iterator it = mOutputs.begin(); it != mOutputs.end(); ++it)
for(unsigned int k = 0; k < (*it)->getOutputCount(); ++k)
aOutput[i++] = (*it)->getOutput(k);
};
//===========================================================================
cFinger::cFinger()
{
// INITIALIZE POSITION OF FINGER:
pos = xSet(0.0, 0.0, 0.0);
// INITIALIZE RADIUS:
radius = 0.003;
// INITIALIZE COLOR (RED DEFAULT COLOR):
color = xSetColor4f(1.0, 0.0, 0.0); // Set red-green-blue components.
// CLEAR FORCES:
clearForce();
// ENABLE FINGER FOR GRAPHIC RENDERING:
enable = true;
}
void ball::doPhysics(float dt)
{
//Euler integration
//F = M*A
float inverseMass=0.1f;
vector2f acc(m_cForce*inverseMass);
vector2f vel(acc*dt);
m_cVelocity = m_cVelocity+vel;
vector2f pos(getPosition2());
auto displacement = m_cVelocity*dt;
pos = pos + m_cVelocity;
m_cVelocity=m_cVelocity*0.9f; //decipate the accumulated velocity over a period of time.
clearForce();
setPosition(pos);
}
void sewClothModel() {
M3DVector3f temp, displacement;
for (int i = 0; i < PTCAMT; i ++) {
m3dLoadVector3(displacement, 0.0f, 0.0f, 0.0f);
clearForce(i);
getInternalForce(i);
//sew together
getTensionForce(i, ClothCenter, 0.01f);
m3dScaleVector3(ClothLnk[i].force, TensionCoefficient);
nextPosition(i);
m3dSubtractVectors3(temp, ClothLnk[i].next_position, ClothLnk[i].position);
m3dAddVectors3(displacement, displacement, temp);
}
m3dScaleVector3(displacement, (GLfloat)800000/PTCAMT);
//printf(" total displace: %.1f %.1f %.1f\n", displacement[0], displacement[1], displacement[2]);
m3dAddVectors3(ClothCenter, ClothCenter, displacement);
//m3dLoadVector3(TotalDisplacement, 0.0f, 0.0f, 0.0f);
refreshPosition();
}
void simulation(void) {
//if (true) {
// sewClothModel();
//} else if (stepCount == 200){
// for (int i = 0; i < PTCAMT; i ++) {
// clearForce(i);
// m3dCopyVector3(ClothLnk[i].next_position, ClothLnk[i].position);
// }
//} else {
for (int i = 0; i < PTCAMT; i ++) {
clearForce(i);
getForce(i);
//if ((i>=44 && i<=46)||(i>=54 && i<=56)||(i>=64 && i<=66))
// printf("[%d].force {%.1f, %.1f, %.1f} \n", i, ClothLnk[i].force[0], ClothLnk[i].force[1], ClothLnk[i].force[2]);
//printf(" ClothLnk[%d].force = {%f, %f, %f} \n", i, ClothLnk[i].force[0], ClothLnk[i].force[1], ClothLnk[i].force[2]);
}
//}
for (int i = 0; i < PTCAMT; i ++) {
nextPosition(i);
}
refreshPosition();
stepCount ++;
}
void RK_CALL manipulator_dynamics_model::computeStateRate(double aTime,const vect_n<double>& aState, vect_n<double>& aStateRate) {
setJointStates(aState);
doMotion();
clearForce();
doForce();
aStateRate.resize(getJointStatesCount());
unsigned int j = 0;
for(std::vector< shared_ptr< gen_coord<double> > >::const_iterator it = mCoords.begin();
it < mCoords.end(); ++it, ++j)
aStateRate[j] = (*it)->q_dot;
for(std::vector< shared_ptr< frame_2D<double> > >::const_iterator it = mFrames2D.begin();
it < mFrames2D.end(); ++it) {
aStateRate[j] = (*it)->Velocity[0]; ++j;
aStateRate[j] = (*it)->Velocity[1]; ++j;
aStateRate[j] = -(*it)->Rotation[1] * (*it)->AngVelocity; ++j;
aStateRate[j] = (*it)->Rotation[0] * (*it)->AngVelocity; ++j;
};
for(std::vector< shared_ptr< frame_3D<double> > >::const_iterator it = mFrames3D.begin();
it < mFrames3D.end(); ++it) {
aStateRate[j] = (*it)->Velocity[0]; ++j;
aStateRate[j] = (*it)->Velocity[1]; ++j;
aStateRate[j] = (*it)->Velocity[2]; ++j;
aStateRate[j] = (*it)->QuatDot[0]; ++j;
aStateRate[j] = (*it)->QuatDot[1]; ++j;
aStateRate[j] = (*it)->QuatDot[2]; ++j;
aStateRate[j] = (*it)->QuatDot[3]; ++j;
};
for(std::vector< shared_ptr< gen_coord<double> > >::const_iterator it = mCoords.begin();
it < mCoords.end(); ++it, ++j)
aStateRate[j] = (*it)->f;
for(std::vector< shared_ptr< frame_2D<double> > >::const_iterator it = mFrames2D.begin();
it < mFrames2D.end(); ++it) {
aStateRate[j] = (*it)->Force[0]; ++j;
aStateRate[j] = (*it)->Force[1]; ++j;
aStateRate[j] = (*it)->Torque; ++j;
};
for(std::vector< shared_ptr< frame_3D<double> > >::const_iterator it = mFrames3D.begin();
it < mFrames3D.end(); ++it) {
aStateRate[j] = (*it)->Force[0]; ++j;
aStateRate[j] = (*it)->Force[1]; ++j;
aStateRate[j] = (*it)->Force[2]; ++j;
aStateRate[j] = (*it)->Torque[0]; ++j;
aStateRate[j] = (*it)->Torque[1]; ++j;
aStateRate[j] = (*it)->Torque[2]; ++j;
};
mat<double,mat_structure::symmetric> Msys(getJointAccelerationsCount());
getMassMatrix(Msys);
try {
mat_vect_adaptor< vect_n<double> > acc_as_mat(aStateRate, getJointAccelerationsCount(), 1, getJointPositionsCount());
linsolve_Cholesky(Msys,acc_as_mat);
} catch(singularity_error& e) { RK_UNUSED(e);
std::stringstream ss; ss << "Mass matrix is singular in the manipulator model '" << getName() << "' at time " << aTime << " seconds.";
throw singularity_error(ss.str());
};
};
