diff --git a/Simulations/MassSpringSystemSimulator.cpp b/Simulations/MassSpringSystemSimulator.cpp index fe9475c..9f7d1cd 100644 --- a/Simulations/MassSpringSystemSimulator.cpp +++ b/Simulations/MassSpringSystemSimulator.cpp @@ -283,17 +283,13 @@ void MassSpringSystemSimulator::applyExternalForce(Vec3 force) //eine Vorstellung: for all Masspoints, update force } -int MassSpringSystemSimulator::LengthCalculator(Vec3 position1, Vec3 position2) { - - Vec3 PosVector = position1 - position2; +float MassSpringSystemSimulator::LengthCalculator(Vec3 vector) +{ //wurzel aus Vektor - int length = sqrt(pow(PosVector.x, 2) + pow(PosVector.y, 2) + pow(PosVector.z, 2)); - + float length = sqrt(vector.x * vector.x + vector.y * vector.y + vector.z * vector.z); return length; } - - Vec3 MassSpringSystemSimulator::calculatePositionTimestepEuler(Vec3 oldPosition, float timestep, Vec3 velocity) { return oldPosition + timestep * velocity; @@ -314,45 +310,44 @@ void MassSpringSystemSimulator::Midpoint(Spring& spring, float timestep) { auto massPoint1 = spring.mp1.lock(); auto massPoint2 = spring.mp2.lock(); - //old position - auto mp = massPoint1->position; - auto mp2 = massPoint2->position; //old Velocity auto mOld_v = massPoint1->velocity; auto m2Old_v = massPoint2->velocity; - Vec3 PosVector = mp - mp2; + Vec3 PosVector = massPoint1->position - massPoint2->position; //Abstand ausrechnen - int d = LengthCalculator(mp, mp2); + float length = LengthCalculator(PosVector); //normalize - Vec3 PosNorm1 = PosVector / d; - Vec3 PosNorm2 = -1 * PosNorm1; - - Vec3 Force = -m_fStiffness * (d - spring.initialLength) * PosNorm1; - Vec3 Force2 = -1 * Force; - - Vec3 oldAcc = calculateAcceleration(Force,m_fMass); - Vec3 oldAcc2 = calculateAcceleration(Force2, m_fMass); + Vec3 normal1 = PosVector / length; + Vec3 normal2 = -1 * normal1; //Midpoint calculator - //Pos of Midstep - Vec3 PosOfMidstep = mp + 0.5 * timestep * mOld_v; - Vec3 PosOfMidstep2 = mp2 + 0.5 * timestep * m2Old_v; - //Vel at Midstep - Vec3 VelAtMidstep = mOld_v + 0.5 * timestep * oldAcc; - Vec3 VelAtMidstep2 = m2Old_v + 0.5 * timestep * oldAcc2; - - Vec3 NewPos = mp + timestep * VelAtMidstep; - Vec3 NewPos2 = mp2 + timestep * VelAtMidstep2; + auto midpointStep1 = MidPointStep(timestep / 2, spring, massPoint1->position, massPoint1->velocity, normal1, length); + auto midpointStep2 = MidPointStep(timestep / 2, spring, massPoint2->position, massPoint2->velocity, normal2, length); - Vec3 NewVel = mOld_v + timestep * oldAcc; - Vec3 NewVel2 = m2Old_v + timestep * oldAcc2; + auto newPosVector = std::get<0>(midpointStep1) - std::get<0>(midpointStep2); + auto newLength = LengthCalculator(newPosVector); + auto newNormal1 = newPosVector / newLength; + auto newNormal2 = -1 * newNormal1; + + auto midpoint1 = MidPointStep(timestep, spring, massPoint1->position, std::get<1>(midpointStep1), newNormal1, newLength); + auto midpoint2 = MidPointStep(timestep, spring, massPoint2->position, std::get<1>(midpointStep2), newNormal2, newLength); + //cout << NewPos; //cout << NewVel; - massPoint1->position = NewPos; - massPoint1->velocity = NewVel; - massPoint2->position = NewPos2; - massPoint2->velocity = NewVel2; + massPoint1->position = std::get<0>(midpoint1); + massPoint1->velocity = std::get<1>(midpoint1); + massPoint2->position = std::get<0>(midpoint2); + massPoint2->velocity = std::get<1>(midpoint2); +} + +std::tuple MassSpringSystemSimulator::MidPointStep(float timestep, const Spring& spring, Vec3 position, Vec3 velocity, Vec3 normal, float length) +{ + Vec3 force = -m_fStiffness * (length - spring.initialLength) * normal; + Vec3 acc = calculateAcceleration(force, m_fMass); + Vec3 pos = position + timestep * velocity; + Vec3 vel = velocity + timestep * acc; + return std::tuple(pos, vel); } void MassSpringSystemSimulator::Euler(Spring& spring, float timestep) @@ -369,11 +364,11 @@ void MassSpringSystemSimulator::Euler(Spring& spring, float timestep) // Force of spring is -k * (l - L) * normalizedVector [for P2 we can take -F1) auto force = -m_fStiffness * (lengthVector - spring.initialLength) * normalized; auto foreP2 = -1 * force; - auto veloc = calculatePositionTimestepEuler(massPoint1->velocity, timestep, calculateAcceleration(force, 10.)); - auto pos = calculatePositionTimestepEuler(massPoint1->position, timestep, veloc); + auto veloc = calculateVelocityTimestepEuler(massPoint1->velocity, timestep, calculateAcceleration(force, m_fMass)); + auto pos = calculatePositionTimestepEuler(massPoint1->position, timestep, massPoint1->velocity); - auto veloc2 = calculateVelocityTimestepEuler(massPoint2->velocity, timestep, calculateAcceleration(foreP2, 10.)); - auto pos2 = calculatePositionTimestepEuler(massPoint2->position, timestep, veloc2); + auto veloc2 = calculateVelocityTimestepEuler(massPoint2->velocity, timestep, calculateAcceleration(foreP2, m_fMass)); + auto pos2 = calculatePositionTimestepEuler(massPoint2->position, timestep, massPoint2->velocity); // Update Positions and Velocity massPoint1->position = pos; diff --git a/Simulations/MassSpringSystemSimulator.h b/Simulations/MassSpringSystemSimulator.h index c2f4878..cb32802 100644 --- a/Simulations/MassSpringSystemSimulator.h +++ b/Simulations/MassSpringSystemSimulator.h @@ -40,7 +40,8 @@ public: void applyExternalForce(Vec3 force); void Midpoint(Spring& spring, float timestep); - int LengthCalculator(Vec3 position1, Vec3 position2); + std::tuple MidPointStep(float timestep, const Spring& spring, Vec3 position, Vec3 velocity, Vec3 normal, float length); + float LengthCalculator(Vec3 vector); Vec3 calculatePositionTimestepEuler(Vec3 oldPosition, float timestep, Vec3 veloctiy); Vec3 calculateVelocityTimestepEuler(Vec3 oldVelocity, float timestep, Vec3 acceleration);