382 lines
11 KiB
C++
382 lines
11 KiB
C++
#include "MassSpringSystemSimulator.h"
|
|
#include <stdio.h>
|
|
|
|
MassSpringSystemSimulator::MassSpringSystemSimulator()
|
|
{
|
|
}
|
|
|
|
const char* MassSpringSystemSimulator::getTestCasesStr()
|
|
{
|
|
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())
|
|
{
|
|
springs.erase(springs.begin() + i);
|
|
continue;
|
|
}
|
|
auto mp1 = sp.mp1.lock();
|
|
auto mp2 = sp.mp2.lock();
|
|
DUC->setUpLighting(Vec3(), 0.4 * Vec3(1, 1, 1), 100, 0.6 * Vec3(0.97, 0.86, 1));
|
|
|
|
DUC->drawSphere(mp1->position, Vec3(0.01));
|
|
DUC->drawSphere(mp2->position, Vec3(0.01));
|
|
|
|
DUC->beginLine();
|
|
DUC->drawLine(mp1->position, Vec3(1,0,0), mp2->position, Vec3(0,1,0));
|
|
DUC->endLine();
|
|
}
|
|
}
|
|
|
|
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)
|
|
{
|
|
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)
|
|
{
|
|
MassPoint masspoint;
|
|
masspoint.position = position;
|
|
masspoint.velocity = Velocity;
|
|
masspoint.isFixed = isFixed;
|
|
masspoints.push_back(std::make_shared<MassPoint>(masspoint));
|
|
|
|
return masspoints.size() - 1;
|
|
}
|
|
|
|
void MassSpringSystemSimulator::addSpring(int masspoint1, int masspoint2, float initialLength)
|
|
{
|
|
auto mp1 = masspoints.at(masspoint1);
|
|
auto mp2 = masspoints.at(masspoint2);
|
|
|
|
Spring spring;
|
|
spring.mp1 = mp1;
|
|
spring.mp2 = mp2;
|
|
spring.initialLength = initialLength;
|
|
|
|
springs.push_back(spring);
|
|
}
|
|
|
|
int MassSpringSystemSimulator::getNumberOfMassPoints()
|
|
{
|
|
return masspoints.size();
|
|
}
|
|
|
|
int MassSpringSystemSimulator::getNumberOfSprings()
|
|
{
|
|
return springs.size();
|
|
}
|
|
|
|
Vec3 MassSpringSystemSimulator::getPositionOfMassPoint(int index)
|
|
{
|
|
auto mp = masspoints.at(index);
|
|
return mp->position;
|
|
}
|
|
|
|
Vec3 MassSpringSystemSimulator::getVelocityOfMassPoint(int index)
|
|
{
|
|
auto mp = masspoints.at(index);
|
|
return mp->velocity;
|
|
}
|
|
|
|
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());
|
|
}
|