fix midpoint

Reviewed-on: #9

Simulations/MassSpringSystemSimulator.cpp

@@ -3,10 +3,6 @@
 
 MassSpringSystemSimulator::MassSpringSystemSimulator()
 {
-	m_iTestCase = 0;
-	m_fMass = 10;
-	m_fStiffness = 40;
-	m_iIntegrator = EULER; 
 }
 
 const char* MassSpringSystemSimulator::getTestCasesStr()
@@ -126,7 +122,7 @@ void MassSpringSystemSimulator::notifyCaseChanged(int testCase)
 
 		break;
 	}
-	
+
 	case 2: {
 		cout << "Demo 3 !\n";
 		reset();
@@ -283,17 +279,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 +306,42 @@ 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 = sqrtf(PosVector.x * PosVector.x + PosVector.y * PosVector.y + PosVector.z * PosVector.z);
 	//normalize
-	Vec3 PosNorm1 = PosVector / d;
-	Vec3 PosNorm2 = -1 * PosNorm1;
-
-	Vec3 Force = -m_fStiffness * (d - springs.at(0).initialLength) * PosNorm1;
-	Vec3 Force2 = -1 * Force;
-
-	Vec3 oldAcc = calculateAcceleration(Force,m_fMass);
-	Vec3 oldAcc2 = calculateAcceleration(Force2, m_fMass);
+	Vec3 normal = PosVector / length;
 
 	//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;
+	std::tuple<Vec3, Vec3> midpointStep1 = MidPointHalfStep(timestep * .5, spring, massPoint1->position, massPoint1->velocity, normal, length);
+	std::tuple<Vec3, Vec3> midpointStep2 = MidPointHalfStep(timestep * .5, spring, massPoint2->position, massPoint2->velocity, -normal, length);
 
-	Vec3 NewVel = mOld_v + timestep * oldAcc;
-	Vec3 NewVel2 = m2Old_v + timestep * oldAcc2;
-	//cout << NewPos;
-	//cout << NewVel;
-	massPoint1->position = NewPos;
-	massPoint1->velocity = NewVel;
-	massPoint2->position = NewPos2;
-	massPoint2->velocity = NewVel2;
+	Vec3 newPosVector = std::get<0>(midpointStep1) - std::get<0>(midpointStep2);
+	float newLength = sqrtf(newPosVector.x * newPosVector.x + newPosVector.y * newPosVector.y + newPosVector.z * newPosVector.z);
+	Vec3 newNormal = newPosVector / newLength;
+
+	std::tuple<Vec3, Vec3> midpoint1 = MidPointStep(timestep, spring, massPoint1->position, massPoint1->velocity, std::get<1>(midpointStep1), newNormal, newLength);
+	std::tuple<Vec3, Vec3> midpoint2 = MidPointStep(timestep, spring, massPoint2->position, massPoint2->velocity, std::get<1>(midpointStep2), -newNormal, newLength);
+	
+	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<Vec3, Vec3> MassSpringSystemSimulator::MidPointHalfStep(double timestep, const Spring& spring, Vec3 position, Vec3 velocity, Vec3 normal, double length)
+{
+	return MidPointStep(timestep, spring, position, velocity, velocity, normal, length);
+}
+
+
+std::tuple<Vec3, Vec3> MassSpringSystemSimulator::MidPointStep(double timestep, const Spring& spring, Vec3 position, Vec3 oldVelo,Vec3 velocity, Vec3 normal, double length)
+{
+	auto force = -m_fStiffness * (length - spring.initialLength) * normal;
+	auto acc = calculateAcceleration(force, m_fMass);
+	auto pos = position + timestep * velocity;
+	auto vel = oldVelo + timestep * acc;
+	return std::tuple<Vec3, Vec3>(pos, vel);
 }
 
 void MassSpringSystemSimulator::Euler(Spring& spring, float timestep)
@@ -369,11 +358,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;

Simulations/MassSpringSystemSimulator.h

@@ -40,7 +40,9 @@ public:
 	void applyExternalForce(Vec3 force);
 
 	void Midpoint(Spring& spring, float timestep);
-	int LengthCalculator(Vec3 position1, Vec3 position2);
+	std::tuple<Vec3, Vec3> MidPointHalfStep(double timestep, const Spring& spring, Vec3 position, Vec3 velocity, Vec3 normal, double length);
+	std::tuple<Vec3, Vec3> MassSpringSystemSimulator::MidPointStep(double timestep, const Spring& spring, Vec3 position, Vec3 oldVelo, Vec3 velocity, Vec3 normal, double length);
+	float LengthCalculator(Vec3 vector);
 
 	Vec3 calculatePositionTimestepEuler(Vec3 oldPosition, float timestep, Vec3 veloctiy);
 	Vec3 calculateVelocityTimestepEuler(Vec3 oldVelocity, float timestep, Vec3 acceleration);