void SpringDamperJoint::applyTo(std::vector<MassPoint> *mpVec) { Vec3d rAB = (*mpVec)[B].r - (*mpVec)[A].r; Vec3d vAB = (*mpVec)[B].v - (*mpVec)[A].v; double rABSquareNorm = rAB.squarenorm(); double fNorm = k * (1 - (l * l) / (rABSquareNorm)) + d * (vAB * rAB / rABSquareNorm); (*mpVec)[A].addForce(rAB * fNorm); (*mpVec)[B].addForce(rAB * (-fNorm)); Vec3d rABfNorm = rAB * fNorm; /*std::cout << "A: (" << pointA->getR().x << ", " << pointA->getR().y << ", " << pointA->getR().z << ")" << std::endl; std::cout << "B: (" << pointB->getR().x << ", " << pointB->getR().y << ", " << pointB->getR().z << ")" << ", dAB: " << sqrt(rABSquareNorm) << std::endl; std::cout << "Force: " << fNorm << ", Vec: " << rABfNorm.x << ", " << rABfNorm.y << ", " << rABfNorm.z << std::endl;*/ }