mirror of
https://github.com/MaSzyna-EU07/maszyna.git
synced 2026-03-22 15:05:03 +01:00
52 lines
1.7 KiB
C++
52 lines
1.7 KiB
C++
/*
|
|
This Source Code Form is subject to the
|
|
terms of the Mozilla Public License, v.
|
|
2.0. If a copy of the MPL was not
|
|
distributed with this file, You can
|
|
obtain one at
|
|
http://mozilla.org/MPL/2.0/.
|
|
*/
|
|
|
|
#include "stdafx.h"
|
|
#include "world/Spring.h"
|
|
|
|
void TSpring::Init(double nKs, double nKd) {
|
|
Ks = nKs;
|
|
Kd = nKd;
|
|
ks = Ks;
|
|
kd = Kd;
|
|
}
|
|
|
|
Math3D::vector3 TSpring::ComputateForces( Math3D::vector3 const &pPosition1, Math3D::vector3 const &pPosition2) {
|
|
|
|
Math3D::vector3 springForce;
|
|
// p1 = &system[spring->p1];
|
|
// p2 = &system[spring->p2];
|
|
// VectorDifference(&p1->pos,&p2->pos,&deltaP); // Vector distance
|
|
auto deltaP = pPosition1 - pPosition2;
|
|
// dist = VectorLength(&deltaP); // Magnitude of deltaP
|
|
auto dist = deltaP.Length();
|
|
if( dist > restLen ) {
|
|
|
|
// Hterm = (dist - spring->restLen) * spring->Ks; // Ks * (dist - rest)
|
|
auto Hterm = ( dist - restLen ) * Ks; // Ks * (dist - rest)
|
|
|
|
// VectorDifference(&p1->v,&p2->v,&deltaV); // Delta Velocity Vector
|
|
auto deltaV = pPosition1 - pPosition2;
|
|
|
|
// Dterm = (DotProduct(&deltaV,&deltaP) * spring->Kd) / dist; // Damping Term
|
|
auto Dterm = (DotProduct(deltaV,deltaP) * Kd) / dist;
|
|
//Dterm = 0;
|
|
|
|
// ScaleVector(&deltaP,1.0f / dist, &springForce); // Normalize Distance Vector
|
|
// ScaleVector(&springForce,-(Hterm + Dterm),&springForce); // Calc Force
|
|
springForce = deltaP / dist * ( -( Hterm + Dterm ));
|
|
// VectorSum(&p1->f,&springForce,&p1->f); // Apply to Particle 1
|
|
// VectorDifference(&p2->f,&springForce,&p2->f); // - Force on Particle 2
|
|
}
|
|
|
|
return springForce;
|
|
}
|
|
|
|
//---------------------------------------------------------------------------
|