fix midpoint

Reviewed-on: #9

Simulations/MassSpringSystemSimulator.cpp

@@ -1,29 +1,44 @@
 #include "MassSpringSystemSimulator.h"
+#include <stdio.h>
 
 MassSpringSystemSimulator::MassSpringSystemSimulator()
 {
-	//Test only
-	//auto first = addMassPoint(Vec3(0, 0, 1), Vec3(0, 0, 0), true);
-	//auto second = addMassPoint(Vec3(0, 0, 0), Vec3(0, 0, 0), true);
-	//addSpring(first, second, 2.0);
 }
 
 const char* MassSpringSystemSimulator::getTestCasesStr()
 {
-	return nullptr;
+	return "Demo1,Demo2,Demo3,Demo4";
 }
 
 void MassSpringSystemSimulator::initUI(DrawingUtilitiesClass* DUC)
 {
 	this->DUC = DUC;
+	switch (m_iTestCase)
+	{
+		case 0:
+			break;
+		case 1:
+			break;
+		case 2:
+			break;
+		default:
+			break;
+	}
 }
 
 void MassSpringSystemSimulator::reset()
 {
+	m_mouse.x = m_mouse.y = 0;
+	m_trackmouse.x = m_trackmouse.y = 0;
+	m_oldtrackmouse.x = m_oldtrackmouse.y = 0;
+
+	springs.clear();
+	masspoints.clear();
 }
 
 void MassSpringSystemSimulator::drawFrame(ID3D11DeviceContext* pd3dImmediateContext)
 {
+	
 	for (size_t i = 0; i < springs.size(); i++) {
 		auto sp = springs.at(i);
 		if (!sp.isValid())
@@ -46,34 +61,171 @@ void MassSpringSystemSimulator::drawFrame(ID3D11DeviceContext* pd3dImmediateCont
 
 void MassSpringSystemSimulator::notifyCaseChanged(int testCase)
 {
+	m_iTestCase = testCase;
+	system("cls");
+	reset();
+
+	switch (m_iTestCase)
+	{
+	case 0: {
+		cout << "Demo 1 !\n";
+		setMass(10);
+		setStiffness(40);
+
+		int first = addMassPoint(Vec3(0, 0, 0), Vec3(-1, 0, 0), true);
+		int second = addMassPoint(Vec3(0, 2, 0), Vec3(1, 0, 0), true);
+
+		addSpring(first, second, 1);
+		cout << "\t -- INITIAL --\n";
+		printSpring(springs.at(0));
+		cout << "--------------------------------------------------" << std::endl;
+
+		//calculate Euler for one step and print results
+		setIntegrator(EULER);
+		cout << "\n\n\t -- EULER RESULT --\n";
+		simulateTimestep(1);
+		printSpring(springs.at(0));
+
+		cout << "--------------------------------------------------" << std::endl;
+
+		reset();
+		first = addMassPoint(Vec3(0, 0, 0), Vec3(-1, 0, 0), true);
+		second = addMassPoint(Vec3(0, 2, 0), Vec3(1, 0, 0), true);
+
+		addSpring(first, second, 1);
+
+		//calculate Midpoint for one step and print results
+		setIntegrator(MIDPOINT);
+		cout << "\n\n\t -- MIDPOINT RESULT --\n";
+		simulateTimestep(1);
+		printSpring(springs.at(0));
+		break;
+	}
+	
+	case 1: {
+		cout << "Demo 2 !\n";
+		reset();
+		int first = addMassPoint(Vec3(0, 0, 0), Vec3(-1, 0, 0), true);
+		int second = addMassPoint(Vec3(0, 2, 0), Vec3(1, 0, 0), true);
+		addSpring(first, second, 1.0);
+
+		cout << "\t -- INITIAL --\n";
+		printSpring(springs.at(0));
+
+		cout << "--------------------------------------------------" << std::endl;
+
+		//calculate Euler for a timestep of 0.005 and print results
+		setIntegrator(EULER);
+		cout << "\n\n\t -- EULER RESULT--\n";
+		simulateTimestep(0.005);
+		printSpring(springs.at(0));
+
+		break;
+	}
+
+	case 2: {
+		cout << "Demo 3 !\n";
+		reset();
+		int first = addMassPoint(Vec3(0, 0, 0), Vec3(-1, 0, 0), true);
+		int second = addMassPoint(Vec3(0, 2, 0), Vec3(1, 0, 0), true);
+		addSpring(first, second, 1.0);
+
+		cout << "\t -- INITIAL --\n";
+		printSpring(springs.at(0));
+
+		cout << "--------------------------------------------------" << std::endl;
+
+		//calculate Midpoint for a timestep of 0.005 and print results
+		setIntegrator(MIDPOINT);
+		cout << "\n\n\t -- MIDPOINT RESULT --\n";
+		simulateTimestep(0.005);
+		printSpring(springs.at(0));
+		break;
+	}
+	case 3: {
+		cout << "Demo 4 !\n";
+		break;
+	}
+	default:
+		break;
+	}
 }
 
-void MassSpringSystemSimulator::externalForcesCalculations(float timeElapsed)
+void MassSpringSystemSimulator::externalForcesCalculations(float timeElapsed) 
 {
+	Point2D mouseDiff;
+	mouseDiff.x = m_trackmouse.x - m_oldtrackmouse.x;
+	mouseDiff.y = m_trackmouse.y - m_oldtrackmouse.y;
+	if (mouseDiff.x != 0 || mouseDiff.y != 0)
+	{
+		Mat4 worldViewInv = Mat4(DUC->g_camera.GetWorldMatrix() * DUC->g_camera.GetViewMatrix());
+		worldViewInv = worldViewInv.inverse();
+		Vec3 inputView = Vec3((float)mouseDiff.x, (float)-mouseDiff.y, 0);
+		Vec3 inputWorld = worldViewInv.transformVectorNormal(inputView);
+		// find a proper scale!
+		float inputScale = 0.001f;
+		inputWorld = inputWorld * inputScale;
+		
+		//m_vfMovableObjectPos = m_vfMovableObjectFinalPos + inputWorld;
+	}
+	else {
+		//m_vfMovableObjectFinalPos = m_vfMovableObjectPos;
+	}
+
 }
 
 void MassSpringSystemSimulator::simulateTimestep(float timeStep)
 {
+	//update current setup for each frame
+	for (size_t i = 0; i < springs.size(); i++) {
+		auto sp = springs.at(i);
+		if (!sp.isValid())
+		{
+			springs.erase(springs.begin() + i);
+			continue;
+		}
+
+		if (m_iIntegrator == EULER) {
+
+			Euler(sp, timeStep);
+		}
+		else if (m_iIntegrator == MIDPOINT) {
+			Midpoint(sp, timeStep);
+		}
+		else if (m_iIntegrator == LEAPFROG) {
+			//TODO: Add Leapfrog
+		}
+
+	}
 }
 
 void MassSpringSystemSimulator::onClick(int x, int y)
 {
+	m_trackmouse.x = x;
+	m_trackmouse.y = y;
 }
 
 void MassSpringSystemSimulator::onMouse(int x, int y)
 {
+	m_oldtrackmouse.x = x;
+	m_oldtrackmouse.y = y;
+	m_trackmouse.x = x;
+	m_trackmouse.y = y;
 }
 
 void MassSpringSystemSimulator::setMass(float mass)
 {
+	m_fMass = mass;
 }
 
 void MassSpringSystemSimulator::setStiffness(float stiffness)
 {
+	m_fStiffness = stiffness;
 }
 
 void MassSpringSystemSimulator::setDampingFactor(float damping)
 {
+	m_fDamping = damping;
 }
 
 int MassSpringSystemSimulator::addMassPoint(Vec3 position, Vec3 Velocity, bool isFixed)
@@ -124,4 +276,106 @@ Vec3 MassSpringSystemSimulator::getVelocityOfMassPoint(int index)
 
 void MassSpringSystemSimulator::applyExternalForce(Vec3 force)
 {
+	//eine Vorstellung: for all Masspoints, update force
+}
+
+float MassSpringSystemSimulator::LengthCalculator(Vec3 vector) 
+{
+	//wurzel aus Vektor
+	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;
+}
+
+Vec3 MassSpringSystemSimulator::calculateVelocityTimestepEuler(Vec3 oldVelocity, float timestep, Vec3 acceleration)
+{
+	return oldVelocity + acceleration * timestep;
+}
+
+Vec3 MassSpringSystemSimulator::calculateAcceleration(Vec3 force, float mass)
+{
+	return force / mass;
+}
+
+void MassSpringSystemSimulator::Midpoint(Spring& spring, float timestep) {
+	//here some implementation about Midpoint
+	auto massPoint1 = spring.mp1.lock();
+	auto massPoint2 = spring.mp2.lock();
+
+	Vec3 PosVector = massPoint1->position - massPoint2->position;
+	//Abstand ausrechnen
+	float length = sqrtf(PosVector.x * PosVector.x + PosVector.y * PosVector.y + PosVector.z * PosVector.z);
+	//normalize
+	Vec3 normal = PosVector / length;
+
+	//Midpoint calculator
+	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 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)
+{
+	auto massPoint1 = spring.mp1.lock();
+	auto massPoint2 = spring.mp2.lock();
+
+	//take old position and send to calculatePositionTimestepEuler
+	auto PosVector = massPoint1->position - massPoint2->position;
+	auto lengthVector = sqrt(PosVector.x * PosVector.x + PosVector.y * PosVector.y + PosVector.z * PosVector.z);
+	auto normalized = PosVector / lengthVector;
+
+	// Actual Calculation
+	// 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 = calculateVelocityTimestepEuler(massPoint1->velocity, timestep, calculateAcceleration(force, m_fMass));
+	auto pos = calculatePositionTimestepEuler(massPoint1->position, timestep, massPoint1->velocity);
+
+	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;
+	massPoint1->velocity = veloc;
+
+	massPoint2->position = pos2;
+	massPoint2->velocity = veloc2;
+}
+
+void MassSpringSystemSimulator::printSpring(const Spring& spring)
+{
+	auto mp1 = spring.mp1.lock();
+	auto mp2 = spring.mp2.lock();
+	printf("Masspoint 1:\nPosition: %s \nVelocity: %s\n\n", mp1->position.toString().c_str(), mp1->velocity.toString().c_str());
+	printf("Masspoint 2:\nPosition: %s \nVelocity: %s\n", mp2->position.toString().c_str(), mp2->velocity.toString().c_str());
 }

Simulations/MassSpringSystemSimulator.h

@@ -1,64 +1,76 @@
-#ifndef MASSSPRINGSYSTEMSIMULATOR_h
+#ifndef MASSSPRINGSYSTEMSIMULATOR_h
-#define MASSSPRINGSYSTEMSIMULATOR_h
+#define MASSSPRINGSYSTEMSIMULATOR_h
-#include "Simulator.h"
+#include "Simulator.h"
-#include "MassPoint.h"
+#include "MassPoint.h"
-#include "Spring.h"
+#include "Spring.h"
-
+
-// Do Not Change
+// Do Not Change
-#define EULER 0
+#define EULER 0
-#define LEAPFROG 1
+#define LEAPFROG 1
-#define MIDPOINT 2
+#define MIDPOINT 2
-// Do Not Change
+// Do Not Change
-
+
-
+
-class MassSpringSystemSimulator:public Simulator{
+class MassSpringSystemSimulator:public Simulator{
-public:
+public:
-	// Construtors
+	// Construtors
-	MassSpringSystemSimulator();
+	MassSpringSystemSimulator();
-	
+	
-	// UI Functions
+	// UI Functions
-	const char * getTestCasesStr();
+	const char * getTestCasesStr();
-	void initUI(DrawingUtilitiesClass * DUC);
+	void initUI(DrawingUtilitiesClass * DUC);
-	void reset();
+	void reset();
-	void drawFrame(ID3D11DeviceContext* pd3dImmediateContext);
+	void drawFrame(ID3D11DeviceContext* pd3dImmediateContext);
-	void notifyCaseChanged(int testCase);
+	void notifyCaseChanged(int testCase);
-	void externalForcesCalculations(float timeElapsed);
+	void externalForcesCalculations(float timeElapsed);
-	void simulateTimestep(float timeStep);
+	void simulateTimestep(float timeStep);
-	void onClick(int x, int y);
+	void onClick(int x, int y);
-	void onMouse(int x, int y);
+	void onMouse(int x, int y);
-
+
-	// Specific Functions
+	// Specific Functions
-	void setMass(float mass);
+	void setMass(float mass);
-	void setStiffness(float stiffness);
+	void setStiffness(float stiffness);
-	void setDampingFactor(float damping);
+	void setDampingFactor(float damping);
-	int addMassPoint(Vec3 position, Vec3 Velocity, bool isFixed);
+	int addMassPoint(Vec3 position, Vec3 Velocity, bool isFixed);
-	void addSpring(int masspoint1, int masspoint2, float initialLength);
+	void addSpring(int masspoint1, int masspoint2, float initialLength);
-	int getNumberOfMassPoints();
+	int getNumberOfMassPoints();
-	int getNumberOfSprings();
+	int getNumberOfSprings();
-	Vec3 getPositionOfMassPoint(int index);
+	Vec3 getPositionOfMassPoint(int index);
-	Vec3 getVelocityOfMassPoint(int index);
+	Vec3 getVelocityOfMassPoint(int index);
 	void applyExternalForce(Vec3 force);
+
+	void Midpoint(Spring& spring, float timestep);
+	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);
+	Vec3 calculateAcceleration(Vec3 acceleration, float mass);
+	void Euler(Spring& spring, float timestep);
+
+	void printSpring(const Spring& spring);
 	
 	// Do Not Change
 	void setIntegrator(int integrator) {
 		m_iIntegrator = integrator;
-	}
+	}
-
+
-private:
+private:
-	// Data Attributes
+	// Data Attributes
 	float m_fMass;
 	float m_fStiffness;
 	float m_fDamping;
-	int m_iIntegrator;
+	int m_iIntegrator;
-
+
-	// UI Attributes
+	// UI Attributes
-	Vec3 m_externalForce;
+	Vec3 m_externalForce;
-	Point2D m_mouse;
+	Point2D m_mouse;
-	Point2D m_trackmouse;
+	Point2D m_trackmouse;
-	Point2D m_oldtrackmouse;
+	Point2D m_oldtrackmouse;
-
+
-	//Mass points and springs
+	//Mass points and springs
-	std::vector<std::shared_ptr<MassPoint>> masspoints;
+	std::vector<std::shared_ptr<MassPoint>> masspoints;
-	std::vector<Spring> springs;
+	std::vector<Spring> springs;
-};
+};
 #endif