Compare commits
5 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
|
2fe3bc4a27 | ||
|
|
17f77bdcf9 | ||
|
c7f66ad59c | ||
|
e1039eec4e | ||
|
|
25f977e43d |
@@ -1,159 +0,0 @@
|
||||
#include "DiffusionSimulator.h"
|
||||
#include "pcgsolver.h"
|
||||
using namespace std;
|
||||
|
||||
Grid::Grid() {
|
||||
}
|
||||
|
||||
|
||||
DiffusionSimulator::DiffusionSimulator()
|
||||
{
|
||||
m_iTestCase = 0;
|
||||
m_vfMovableObjectPos = Vec3();
|
||||
m_vfMovableObjectFinalPos = Vec3();
|
||||
m_vfRotate = Vec3();
|
||||
// to be implemented
|
||||
}
|
||||
|
||||
const char * DiffusionSimulator::getTestCasesStr(){
|
||||
return "Explicit_solver, Implicit_solver";
|
||||
}
|
||||
|
||||
void DiffusionSimulator::reset(){
|
||||
m_mouse.x = m_mouse.y = 0;
|
||||
m_trackmouse.x = m_trackmouse.y = 0;
|
||||
m_oldtrackmouse.x = m_oldtrackmouse.y = 0;
|
||||
|
||||
}
|
||||
|
||||
void DiffusionSimulator::initUI(DrawingUtilitiesClass * DUC)
|
||||
{
|
||||
this->DUC = DUC;
|
||||
// to be implemented
|
||||
}
|
||||
|
||||
void DiffusionSimulator::notifyCaseChanged(int testCase)
|
||||
{
|
||||
m_iTestCase = testCase;
|
||||
m_vfMovableObjectPos = Vec3(0, 0, 0);
|
||||
m_vfRotate = Vec3(0, 0, 0);
|
||||
//
|
||||
//to be implemented
|
||||
//
|
||||
switch (m_iTestCase)
|
||||
{
|
||||
case 0:
|
||||
cout << "Explicit solver!\n";
|
||||
break;
|
||||
case 1:
|
||||
cout << "Implicit solver!\n";
|
||||
break;
|
||||
default:
|
||||
cout << "Empty Test!\n";
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
Grid* DiffusionSimulator::diffuseTemperatureExplicit() {//add your own parameters
|
||||
Grid* newT = new Grid();
|
||||
// to be implemented
|
||||
//make sure that the temperature in boundary cells stays zero
|
||||
return newT;
|
||||
}
|
||||
|
||||
void setupB(std::vector<Real>& b) {//add your own parameters
|
||||
// to be implemented
|
||||
//set vector B[sizeX*sizeY]
|
||||
for (int i = 0; i < 25; i++) {
|
||||
b.at(i) = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void fillT() {//add your own parameters
|
||||
// to be implemented
|
||||
//fill T with solved vector x
|
||||
//make sure that the temperature in boundary cells stays zero
|
||||
}
|
||||
|
||||
void setupA(SparseMatrix<Real>& A, double factor) {//add your own parameters
|
||||
// to be implemented
|
||||
//setup Matrix A[sizeX*sizeY*sizeZ, sizeX*sizeY*sizeZ]
|
||||
// set with: A.set_element( index1, index2 , value );
|
||||
// if needed, read with: A(index1, index2);
|
||||
// avoid zero rows in A -> set the diagonal value for boundary cells to 1.0
|
||||
for (int i = 0; i < 25; i++) {
|
||||
A.set_element(i, i, 1); // set diagonal
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void DiffusionSimulator::diffuseTemperatureImplicit() {//add your own parameters
|
||||
// solve A T = b
|
||||
// to be implemented
|
||||
const int N = 25;//N = sizeX*sizeY*sizeZ
|
||||
SparseMatrix<Real> *A = new SparseMatrix<Real> (N);
|
||||
std::vector<Real> *b = new std::vector<Real>(N);
|
||||
|
||||
setupA(*A, 0.1);
|
||||
setupB(*b);
|
||||
|
||||
// perform solve
|
||||
Real pcg_target_residual = 1e-05;
|
||||
Real pcg_max_iterations = 1000;
|
||||
Real ret_pcg_residual = 1e10;
|
||||
int ret_pcg_iterations = -1;
|
||||
|
||||
SparsePCGSolver<Real> solver;
|
||||
solver.set_solver_parameters(pcg_target_residual, pcg_max_iterations, 0.97, 0.25);
|
||||
|
||||
std::vector<Real> x(N);
|
||||
for (int j = 0; j < N; ++j) { x[j] = 0.; }
|
||||
|
||||
// preconditioners: 0 off, 1 diagonal, 2 incomplete cholesky
|
||||
solver.solve(*A, *b, x, ret_pcg_residual, ret_pcg_iterations, 0);
|
||||
// x contains the new temperature values
|
||||
fillT();//copy x to T
|
||||
}
|
||||
|
||||
|
||||
|
||||
void DiffusionSimulator::simulateTimestep(float timeStep)
|
||||
{
|
||||
// to be implemented
|
||||
// update current setup for each frame
|
||||
switch (m_iTestCase)
|
||||
{
|
||||
case 0:
|
||||
T = diffuseTemperatureExplicit();
|
||||
break;
|
||||
case 1:
|
||||
diffuseTemperatureImplicit();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void DiffusionSimulator::drawObjects()
|
||||
{
|
||||
// to be implemented
|
||||
//visualization
|
||||
}
|
||||
|
||||
|
||||
void DiffusionSimulator::drawFrame(ID3D11DeviceContext* pd3dImmediateContext)
|
||||
{
|
||||
drawObjects();
|
||||
}
|
||||
|
||||
void DiffusionSimulator::onClick(int x, int y)
|
||||
{
|
||||
m_trackmouse.x = x;
|
||||
m_trackmouse.y = y;
|
||||
}
|
||||
|
||||
void DiffusionSimulator::onMouse(int x, int y)
|
||||
{
|
||||
m_oldtrackmouse.x = x;
|
||||
m_oldtrackmouse.y = y;
|
||||
m_trackmouse.x = x;
|
||||
m_trackmouse.y = y;
|
||||
}
|
||||
@@ -1,51 +0,0 @@
|
||||
#ifndef DIFFUSIONSIMULATOR_h
|
||||
#define DIFFUSIONSIMULATOR_h
|
||||
|
||||
#include "Simulator.h"
|
||||
#include "vectorbase.h"
|
||||
|
||||
//impement your own grid class for saving grid data
|
||||
class Grid {
|
||||
public:
|
||||
// Construtors
|
||||
Grid();
|
||||
|
||||
|
||||
private:
|
||||
// Attributes
|
||||
};
|
||||
|
||||
|
||||
|
||||
class DiffusionSimulator:public Simulator{
|
||||
public:
|
||||
// Construtors
|
||||
DiffusionSimulator();
|
||||
|
||||
// Functions
|
||||
const char * getTestCasesStr();
|
||||
void initUI(DrawingUtilitiesClass * DUC);
|
||||
void reset();
|
||||
void drawFrame(ID3D11DeviceContext* pd3dImmediateContext);
|
||||
void notifyCaseChanged(int testCase);
|
||||
void simulateTimestep(float timeStep);
|
||||
void externalForcesCalculations(float timeElapsed) {};
|
||||
void onClick(int x, int y);
|
||||
void onMouse(int x, int y);
|
||||
// Specific Functions
|
||||
void drawObjects();
|
||||
Grid* diffuseTemperatureExplicit();
|
||||
void diffuseTemperatureImplicit();
|
||||
|
||||
private:
|
||||
// Attributes
|
||||
Vec3 m_vfMovableObjectPos;
|
||||
Vec3 m_vfMovableObjectFinalPos;
|
||||
Vec3 m_vfRotate;
|
||||
Point2D m_mouse;
|
||||
Point2D m_trackmouse;
|
||||
Point2D m_oldtrackmouse;
|
||||
Grid *T; //save results of every time step
|
||||
};
|
||||
|
||||
#endif
|
||||
46
Simulations/RigidBodySystemSimulator.h
Normal file
46
Simulations/RigidBodySystemSimulator.h
Normal file
@@ -0,0 +1,46 @@
|
||||
#ifndef RIGIDBODYSYSTEMSIMULATOR_h
|
||||
#define RIGIDBODYSYSTEMSIMULATOR_h
|
||||
#include "Simulator.h"
|
||||
//add your header for your rigid body system, for e.g.,
|
||||
//#include "rigidBodySystem.h"
|
||||
|
||||
#define TESTCASEUSEDTORUNTEST 2
|
||||
|
||||
class RigidBodySystemSimulator:public Simulator{
|
||||
public:
|
||||
// Construtors
|
||||
RigidBodySystemSimulator();
|
||||
|
||||
// Functions
|
||||
const char * getTestCasesStr();
|
||||
void initUI(DrawingUtilitiesClass * DUC);
|
||||
void reset();
|
||||
void drawFrame(ID3D11DeviceContext* pd3dImmediateContext);
|
||||
void notifyCaseChanged(int testCase);
|
||||
void externalForcesCalculations(float timeElapsed);
|
||||
void simulateTimestep(float timeStep);
|
||||
void onClick(int x, int y);
|
||||
void onMouse(int x, int y);
|
||||
|
||||
// ExtraFunctions
|
||||
int getNumberOfRigidBodies();
|
||||
Vec3 getPositionOfRigidBody(int i);
|
||||
Vec3 getLinearVelocityOfRigidBody(int i);
|
||||
Vec3 getAngularVelocityOfRigidBody(int i);
|
||||
void applyForceOnBody(int i, Vec3 loc, Vec3 force);
|
||||
void addRigidBody(Vec3 position, Vec3 size, int mass);
|
||||
void setOrientationOf(int i,Quat orientation);
|
||||
void setVelocityOf(int i, Vec3 velocity);
|
||||
|
||||
private:
|
||||
// Attributes
|
||||
// add your RigidBodySystem data members, for e.g.,
|
||||
// RigidBodySystem * m_pRigidBodySystem;
|
||||
Vec3 m_externalForce;
|
||||
|
||||
// UI Attributes
|
||||
Point2D m_mouse;
|
||||
Point2D m_trackmouse;
|
||||
Point2D m_oldtrackmouse;
|
||||
};
|
||||
#endif
|
||||
@@ -1,11 +0,0 @@
|
||||
#include "SphereSystemSimulator.h"
|
||||
|
||||
std::function<float(float)> SphereSystemSimulator::m_Kernels[5] = {
|
||||
[](float x) {return 1.0f; }, // Constant, m_iKernel = 0
|
||||
[](float x) {return 1.0f - x; }, // Linear, m_iKernel = 1, as given in the exercise Sheet, x = d/2r
|
||||
[](float x) {return (1.0f - x)*(1.0f - x); }, // Quadratic, m_iKernel = 2
|
||||
[](float x) {return 1.0f / (x)-1.0f; }, // Weak Electric Charge, m_iKernel = 3
|
||||
[](float x) {return 1.0f / (x*x) - 1.0f; }, // Electric Charge, m_iKernel = 4
|
||||
};
|
||||
|
||||
// SphereSystemSimulator member functions
|
||||
@@ -1,49 +0,0 @@
|
||||
#ifndef SPHSYSTEMSIMULATOR_h
|
||||
#define SPHSYSTEMSIMULATOR_h
|
||||
#include "Simulator.h"
|
||||
//#include "spheresystem.h", add your sphere system header file
|
||||
|
||||
#define NAIVEACC 0
|
||||
#define GRIDACC 1
|
||||
|
||||
class SphereSystemSimulator:public Simulator{
|
||||
public:
|
||||
// Construtors
|
||||
SphereSystemSimulator();
|
||||
// Functions
|
||||
const char * getTestCasesStr();
|
||||
void initUI(DrawingUtilitiesClass * DUC);
|
||||
void reset();
|
||||
void drawFrame(ID3D11DeviceContext* pd3dImmediateContext);
|
||||
void notifyCaseChanged(int testCase);
|
||||
void externalForcesCalculations(float timeElapsed);
|
||||
void simulateTimestep(float timeStep);
|
||||
void onClick(int x, int y);
|
||||
void onMouse(int x, int y);
|
||||
|
||||
protected:
|
||||
// Attributes
|
||||
Vec3 externalForce;
|
||||
Point2D m_mouse;
|
||||
Point2D m_trackmouse;
|
||||
Point2D m_oldtrackmouse;
|
||||
float m_fMass;
|
||||
float m_fRadius;
|
||||
float m_fForceScaling;
|
||||
float m_fDamping;
|
||||
int m_iNumSpheres;
|
||||
|
||||
int m_iKernel; // index of the m_Kernels[5], more detials in SphereSystemSimulator.cpp
|
||||
static std::function<float(float)> m_Kernels[5];
|
||||
|
||||
int m_iAccelerator; // switch between NAIVEACC and GRIDACC, (optionally, KDTREEACC, 2)
|
||||
|
||||
//SphereSystem * m_pSphereSystem; // add your own sphere system member!
|
||||
// for Demo 3 only:
|
||||
// you will need multiple SphereSystem objects to do comparisons in Demo 3
|
||||
// m_iAccelerator should be ignored.
|
||||
// SphereSystem * m_pSphereSystemGrid;
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
477
Simulations/collisionDetect.h
Normal file
477
Simulations/collisionDetect.h
Normal file
@@ -0,0 +1,477 @@
|
||||
// header file:
|
||||
#include <DirectXMath.h>
|
||||
#include <Vector>
|
||||
using namespace DirectX;
|
||||
|
||||
// the return structure, with these values, you should be able to calculate the impulse
|
||||
// the depth shouldn't be used in your impulse calculation, it is a redundant value
|
||||
// if the normalWorld == XMVectorZero(), no collision
|
||||
struct CollisionInfo{
|
||||
bool isValid; // whether there is a collision point, true for yes
|
||||
GamePhysics::Vec3 collisionPointWorld; // the position of the collision point in world space
|
||||
GamePhysics::Vec3 normalWorld; // the direction of the impulse to A, negative of the collision face of A
|
||||
float depth; // the distance of the collision point to the surface, not necessary.
|
||||
};
|
||||
|
||||
// tool data structures/functions called by the collision detection method, you can ignore the details here
|
||||
namespace collisionTools{
|
||||
struct Projection{
|
||||
float min, max;
|
||||
};
|
||||
|
||||
|
||||
inline std::vector<XMVECTOR> discritizeObject(const XMMATRIX& obj2World)
|
||||
{
|
||||
const XMVECTOR centerWorld = XMVector3Transform(XMVectorZero(), obj2World);
|
||||
XMVECTOR edges[3];
|
||||
std::vector<XMVECTOR> results;
|
||||
for (int precession = 0.1; precession <= 0.5; precession += 0.1)
|
||||
{
|
||||
for (size_t i = 0; i < 3; ++i)
|
||||
edges[i] = XMVector3TransformNormal(XMVectorSetByIndex(XMVectorZero(), precession, i), obj2World);
|
||||
results.push_back(centerWorld - edges[0] - edges[1] - edges[2]);
|
||||
results.push_back(centerWorld + edges[0] - edges[1] - edges[2]);
|
||||
results.push_back(centerWorld - edges[0] + edges[1] - edges[2]);
|
||||
results.push_back(centerWorld + edges[0] + edges[1] - edges[2]);
|
||||
results.push_back(centerWorld - edges[0] - edges[1] + edges[2]);
|
||||
results.push_back(centerWorld + edges[0] - edges[1] + edges[2]);
|
||||
results.push_back(centerWorld - edges[0] + edges[1] + edges[2]);
|
||||
results.push_back(centerWorld + edges[0] + edges[1] + edges[2]);
|
||||
}
|
||||
}
|
||||
|
||||
inline XMVECTOR getVectorConnnectingCenters(const XMMATRIX& obj2World_A, const XMMATRIX& obj2World_B)
|
||||
{
|
||||
const XMVECTOR centerWorld_A = XMVector3Transform(XMVectorZero(), obj2World_A);
|
||||
const XMVECTOR centerWorld_B = XMVector3Transform(XMVectorZero(), obj2World_B);
|
||||
return centerWorld_B - centerWorld_A;
|
||||
|
||||
}
|
||||
|
||||
// Get Corners
|
||||
inline std::vector<XMVECTOR> getCorners(const XMMATRIX& obj2World)
|
||||
{
|
||||
const XMVECTOR centerWorld = XMVector3Transform(XMVectorZero(), obj2World);
|
||||
XMVECTOR edges[3];
|
||||
for (size_t i = 0; i < 3; ++i)
|
||||
edges[i] = XMVector3TransformNormal(XMVectorSetByIndex(XMVectorZero(), 0.5f, i), obj2World);
|
||||
std::vector<XMVECTOR> results;
|
||||
results.push_back(centerWorld - edges[0] - edges[1] - edges[2]);
|
||||
results.push_back(centerWorld + edges[0] - edges[1] - edges[2]);
|
||||
results.push_back(centerWorld - edges[0] + edges[1] - edges[2]);
|
||||
results.push_back(centerWorld + edges[0] + edges[1] - edges[2]); // this +,+,-
|
||||
results.push_back(centerWorld - edges[0] - edges[1] + edges[2]);
|
||||
results.push_back(centerWorld + edges[0] - edges[1] + edges[2]); //this +,-,+
|
||||
results.push_back(centerWorld - edges[0] + edges[1] + edges[2]); //this -,+,+
|
||||
results.push_back(centerWorld + edges[0] + edges[1] + edges[2]);//this +,+,+
|
||||
return results;
|
||||
}
|
||||
|
||||
// Get Rigid Box Size
|
||||
inline XMVECTOR getBoxSize(const XMMATRIX& obj2World)
|
||||
{
|
||||
XMVECTOR size = XMVectorZero();
|
||||
XMVECTOR edges[3];
|
||||
for (size_t i = 0; i < 3; ++i){
|
||||
edges[i] = XMVector3TransformNormal(XMVectorSetByIndex(XMVectorZero(), 0.5f, i), obj2World);
|
||||
XMVECTOR length = XMVector3Length(edges[i]);
|
||||
|
||||
size = XMVectorSetByIndex(size, 2.0f*XMVectorGetByIndex(length, 0), i);
|
||||
}
|
||||
return size;
|
||||
}
|
||||
|
||||
// Get important Edges
|
||||
inline std::vector<XMVECTOR> getImportantEdges(const XMMATRIX& obj2World)
|
||||
{
|
||||
XMVECTOR xaxis = XMVectorSet(1, 0, 0, 1);
|
||||
XMVECTOR yaxis = XMVectorSet(0, 1, 0, 1);
|
||||
XMVECTOR zaxis = XMVectorSet(0, 0, 1, 1);
|
||||
XMVECTOR edge1 = XMVector3TransformNormal(xaxis, obj2World);
|
||||
XMVECTOR edge2 = XMVector3TransformNormal(yaxis, obj2World);
|
||||
XMVECTOR edge3 = XMVector3TransformNormal(zaxis, obj2World);
|
||||
std::vector<XMVECTOR> results;
|
||||
results.push_back(edge1);
|
||||
results.push_back(edge2);
|
||||
results.push_back(edge3);
|
||||
return results;
|
||||
}
|
||||
|
||||
// Get the Normal to the faces
|
||||
inline std::vector<XMVECTOR> getAxisNormalToFaces(const XMMATRIX& obj2World)
|
||||
{
|
||||
std::vector<XMVECTOR> edges;
|
||||
XMVECTOR xaxis = XMVectorSet(1, 0, 0, 1);
|
||||
XMVECTOR yaxis = XMVectorSet(0, 1, 0, 1);
|
||||
XMVECTOR zaxis = XMVectorSet(0, 0, 1, 1);
|
||||
XMVECTOR edge1 = XMVector3Normalize(XMVector3TransformNormal(xaxis, obj2World));
|
||||
XMVECTOR edge2 = XMVector3Normalize(XMVector3TransformNormal(yaxis, obj2World));
|
||||
XMVECTOR edge3 = XMVector3Normalize(XMVector3TransformNormal(zaxis, obj2World));
|
||||
std::vector<XMVECTOR> results;
|
||||
edges.push_back(edge1);
|
||||
edges.push_back(edge2);
|
||||
edges.push_back(edge3);
|
||||
return edges;
|
||||
}
|
||||
|
||||
|
||||
// Get the pair of edges
|
||||
inline std::vector<XMVECTOR> getPairOfEdges(const XMMATRIX& obj2World_A, const XMMATRIX& obj2World_B)
|
||||
{
|
||||
std::vector<XMVECTOR> edges1 = getAxisNormalToFaces(obj2World_A);
|
||||
std::vector<XMVECTOR> edges2 = getAxisNormalToFaces(obj2World_B);
|
||||
|
||||
std::vector<XMVECTOR> results;
|
||||
for (int i = 0; i < edges1.size(); i++)
|
||||
{
|
||||
for (int j = 0; j<edges2.size(); j++)
|
||||
{
|
||||
XMVECTOR vector = XMVector3Cross(edges1[i], edges2[j]);
|
||||
if (XMVectorGetX(XMVector3Length(vector)) > 0)
|
||||
results.push_back(XMVector3Normalize(vector));
|
||||
}
|
||||
}
|
||||
return results;
|
||||
}
|
||||
|
||||
// project a shape on an axis
|
||||
inline Projection project(const XMMATRIX& obj2World, XMVECTOR axis)
|
||||
{
|
||||
// Get corners
|
||||
std::vector<XMVECTOR> cornersWorld = getCorners(obj2World);
|
||||
float min = XMVectorGetX(XMVector3Dot(cornersWorld[0], axis));
|
||||
float max = min;
|
||||
for (int i = 1; i < cornersWorld.size(); i++)
|
||||
{
|
||||
float p = XMVectorGetX(XMVector3Dot(cornersWorld[i], axis));
|
||||
if (p < min) {
|
||||
min = p;
|
||||
}
|
||||
else if (p > max) {
|
||||
max = p;
|
||||
}
|
||||
}
|
||||
Projection projection;
|
||||
projection.max = max;
|
||||
projection.min = min;
|
||||
return projection;
|
||||
}
|
||||
|
||||
inline bool overlap(Projection p1, Projection p2)
|
||||
{
|
||||
return !((p1.max > p2.max && p1.min > p2.max) || (p2.max > p1.max && p2.min > p1.max));
|
||||
}
|
||||
|
||||
inline float getOverlap(Projection p1, Projection p2)
|
||||
{
|
||||
return XMMin(p1.max, p2.max) - XMMax(p1.min, p2.min);
|
||||
}
|
||||
|
||||
static inline XMVECTOR contactPoint(
|
||||
const XMVECTOR &pOne,
|
||||
const XMVECTOR &dOne,
|
||||
float oneSize,
|
||||
const XMVECTOR &pTwo,
|
||||
const XMVECTOR &dTwo,
|
||||
float twoSize,
|
||||
|
||||
// If this is true, and the contact point is outside
|
||||
// the edge (in the case of an edge-face contact) then
|
||||
// we use one's midpoint, otherwise we use two's.
|
||||
bool useOne)
|
||||
{
|
||||
XMVECTOR toSt, cOne, cTwo;
|
||||
float dpStaOne, dpStaTwo, dpOneTwo, smOne, smTwo;
|
||||
float denom, mua, mub;
|
||||
|
||||
smOne = XMVectorGetX(XMVector3LengthSq(dOne));
|
||||
smTwo = XMVectorGetX(XMVector3LengthSq(dTwo));
|
||||
dpOneTwo = XMVectorGetX(XMVector3Dot(dTwo, dOne));
|
||||
|
||||
toSt = pOne - pTwo;
|
||||
dpStaOne = XMVectorGetX(XMVector3Dot(dOne, toSt));
|
||||
dpStaTwo = XMVectorGetX(XMVector3Dot(dTwo, toSt));
|
||||
|
||||
denom = smOne * smTwo - dpOneTwo * dpOneTwo;
|
||||
|
||||
// Zero denominator indicates parrallel lines
|
||||
if (abs(denom) < 0.0001f) {
|
||||
return useOne ? pOne : pTwo;
|
||||
}
|
||||
|
||||
mua = (dpOneTwo * dpStaTwo - smTwo * dpStaOne) / denom;
|
||||
mub = (smOne * dpStaTwo - dpOneTwo * dpStaOne) / denom;
|
||||
|
||||
// If either of the edges has the nearest point out
|
||||
// of bounds, then the edges aren't crossed, we have
|
||||
// an edge-face contact. Our point is on the edge, which
|
||||
// we know from the useOne parameter.
|
||||
if (mua > oneSize ||
|
||||
mua < -oneSize ||
|
||||
mub > twoSize ||
|
||||
mub < -twoSize)
|
||||
{
|
||||
return useOne ? pOne : pTwo;
|
||||
}
|
||||
else
|
||||
{
|
||||
cOne = pOne + dOne * mua;
|
||||
cTwo = pTwo + dTwo * mub;
|
||||
|
||||
return cOne * 0.5 + cTwo * 0.5;
|
||||
}
|
||||
}
|
||||
|
||||
inline XMVECTOR handleVertexToface(const XMMATRIX& obj2World, const XMVECTOR& toCenter)
|
||||
{
|
||||
std::vector<XMVECTOR> corners = getCorners(obj2World);
|
||||
float min = 1000;
|
||||
XMVECTOR vertex;
|
||||
for (int i = 0; i < corners.size(); i++)
|
||||
{
|
||||
float value = XMVectorGetX(XMVector3Dot(corners[i], toCenter));
|
||||
if (value < min)
|
||||
{
|
||||
vertex = corners[i];
|
||||
min = value;
|
||||
}
|
||||
}
|
||||
|
||||
return vertex;
|
||||
}
|
||||
|
||||
|
||||
inline CollisionInfo checkCollisionSATHelper(const XMMATRIX& obj2World_A, const XMMATRIX& obj2World_B, XMVECTOR size_A, XMVECTOR size_B)
|
||||
{
|
||||
CollisionInfo info;
|
||||
info.isValid = false;
|
||||
XMVECTOR collisionPoint = XMVectorZero();
|
||||
float smallOverlap = 10000.0f;
|
||||
XMVECTOR axis;
|
||||
int index;
|
||||
int fromWhere = -1;
|
||||
bool bestSingleAxis = false;
|
||||
XMVECTOR toCenter = getVectorConnnectingCenters(obj2World_A, obj2World_B);
|
||||
std::vector<XMVECTOR> axes1 = getAxisNormalToFaces(obj2World_A);
|
||||
std::vector<XMVECTOR> axes2 = getAxisNormalToFaces(obj2World_B);
|
||||
std::vector<XMVECTOR> axes3 = getPairOfEdges(obj2World_A, obj2World_B);
|
||||
// loop over the axes1
|
||||
for (int i = 0; i < axes1.size(); i++) {
|
||||
// project both shapes onto the axis
|
||||
Projection p1 = project(obj2World_A, axes1[i]);
|
||||
Projection p2 = project(obj2World_B, axes1[i]);
|
||||
// do the projections overlap?
|
||||
if (!overlap(p1, p2)) {
|
||||
// then we can guarantee that the shapes do not overlap
|
||||
return info;
|
||||
}
|
||||
else{
|
||||
// get the overlap
|
||||
float o = getOverlap(p1, p2);
|
||||
// check for minimum
|
||||
if (o < smallOverlap) {
|
||||
// then set this one as the smallest
|
||||
smallOverlap = o;
|
||||
axis = axes1[i];
|
||||
index = i;
|
||||
fromWhere = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
// loop over the axes2
|
||||
for (int i = 0; i < axes2.size(); i++) {
|
||||
// project both shapes onto the axis
|
||||
Projection p1 = project(obj2World_A, axes2[i]);
|
||||
Projection p2 = project(obj2World_B, axes2[i]);
|
||||
// do the projections overlap?
|
||||
if (!overlap(p1, p2)) {
|
||||
// then we can guarantee that the shapes do not overlap
|
||||
return info;
|
||||
}
|
||||
else{
|
||||
// get the overlap
|
||||
float o = getOverlap(p1, p2);
|
||||
// check for minimum
|
||||
if (o < smallOverlap) {
|
||||
// then set this one as the smallest
|
||||
smallOverlap = o;
|
||||
axis = axes2[i];
|
||||
index = i;
|
||||
fromWhere = 1;
|
||||
bestSingleAxis = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
int whichEdges = 0;
|
||||
// loop over the axes3
|
||||
for (int i = 0; i < axes3.size(); i++) {
|
||||
// project both shapes onto the axis
|
||||
Projection p1 = project(obj2World_A, axes3[i]);
|
||||
Projection p2 = project(obj2World_B, axes3[i]);
|
||||
// do the projections overlap?
|
||||
if (!overlap(p1, p2)) {
|
||||
// then we can guarantee that the shapes do not overlap
|
||||
return info;
|
||||
}
|
||||
else{
|
||||
// get the overlap
|
||||
float o = getOverlap(p1, p2);
|
||||
// check for minimum
|
||||
if (o < smallOverlap) {
|
||||
// then set this one as the smallest
|
||||
smallOverlap = o;
|
||||
axis = axes3[i];
|
||||
index = i;
|
||||
whichEdges = i;
|
||||
fromWhere = 2;
|
||||
}
|
||||
}
|
||||
}
|
||||
// if we get here then we know that every axis had overlap on it
|
||||
// so we can guarantee an intersection
|
||||
XMVECTOR normal;
|
||||
switch (fromWhere){
|
||||
case 0:{
|
||||
normal = axis;
|
||||
if (XMVectorGetX(XMVector3Dot(axis, toCenter)) <= 0)
|
||||
{
|
||||
normal = normal * -1.0f;
|
||||
}
|
||||
collisionPoint = handleVertexToface(obj2World_B, toCenter);
|
||||
}break;
|
||||
case 1:{
|
||||
normal = axis;
|
||||
if (XMVectorGetX(XMVector3Dot(axis, toCenter)) <= 0)
|
||||
{
|
||||
normal = normal * -1.0f;
|
||||
}
|
||||
collisionPoint = handleVertexToface(obj2World_A, toCenter*-1);
|
||||
}break;
|
||||
case 2:{
|
||||
XMVECTOR axis = XMVector3Normalize(XMVector3Cross(axes1[whichEdges / 3], axes2[whichEdges % 3]));
|
||||
normal = axis;
|
||||
if (XMVectorGetX(XMVector3Dot(axis, toCenter)) <= 0)
|
||||
{
|
||||
normal = normal * -1.0f;
|
||||
}
|
||||
XMVECTOR ptOnOneEdge = XMVectorSet(0.5, 0.5, 0.5, 1);
|
||||
XMVECTOR ptOnTwoEdge = XMVectorSet(0.5, 0.5, 0.5, 1);
|
||||
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
if (i == whichEdges / 3) ptOnOneEdge = XMVectorSetByIndex(ptOnOneEdge, 0, i);
|
||||
else if (XMVectorGetX(XMVector3Dot(axes1[i], normal)) < 0) ptOnOneEdge = XMVectorSetByIndex(ptOnOneEdge, -XMVectorGetByIndex(ptOnOneEdge, i), i);
|
||||
|
||||
if (i == whichEdges % 3) ptOnTwoEdge = XMVectorSetByIndex(ptOnTwoEdge, 0, i);
|
||||
else if (XMVectorGetX(XMVector3Dot(axes2[i], normal)) > 0) ptOnTwoEdge = XMVectorSetByIndex(ptOnTwoEdge, -XMVectorGetByIndex(ptOnTwoEdge, i), i);
|
||||
}
|
||||
ptOnOneEdge = XMVector3Transform(ptOnOneEdge, obj2World_A);
|
||||
ptOnTwoEdge = XMVector3Transform(ptOnTwoEdge, obj2World_B);
|
||||
collisionPoint = contactPoint(ptOnOneEdge,
|
||||
axes1[whichEdges / 3],
|
||||
(float)XMVectorGetByIndex(size_A, (whichEdges / 3)),
|
||||
ptOnTwoEdge,
|
||||
axes2[whichEdges % 3],
|
||||
XMVectorGetByIndex(size_B, (whichEdges % 3)),
|
||||
bestSingleAxis);
|
||||
}break;
|
||||
}
|
||||
|
||||
|
||||
info.isValid = true;
|
||||
info.collisionPointWorld = collisionPoint;
|
||||
info.depth = smallOverlap;
|
||||
info.normalWorld = normal*-1;
|
||||
return info;
|
||||
}
|
||||
}
|
||||
|
||||
/* params:
|
||||
obj2World_A, the transfer matrix from object space of A to the world space
|
||||
obj2World_B, the transfer matrix from object space of B to the world space
|
||||
*/
|
||||
inline CollisionInfo checkCollisionSAT(GamePhysics::Mat4& obj2World_A, GamePhysics::Mat4& obj2World_B) {
|
||||
using namespace collisionTools;
|
||||
XMMATRIX MatA = obj2World_A.toDirectXMatrix(), MatB = obj2World_B.toDirectXMatrix();
|
||||
XMVECTOR calSizeA = getBoxSize(MatA);
|
||||
XMVECTOR calSizeB = getBoxSize(MatB);
|
||||
|
||||
return checkCollisionSATHelper(MatA, MatB, calSizeA, calSizeB);
|
||||
}
|
||||
|
||||
// example of using the checkCollisionSAT function
|
||||
inline void testCheckCollision(int caseid){
|
||||
|
||||
if (caseid == 1){// simple examples, suppose that boxes A and B are cubes and have no rotation
|
||||
GamePhysics::Mat4 AM; AM.initTranslation(1.0, 1.0, 1.0);// box A at (1.0,1.0,1.0)
|
||||
GamePhysics::Mat4 BM; BM.initTranslation(2.0, 2.0, 2.0); //box B at (2.0,2.0,2.0)
|
||||
|
||||
// check for collision
|
||||
CollisionInfo simpletest = checkCollisionSAT(AM, BM);// should find out a collision here
|
||||
if (!simpletest.isValid)
|
||||
std::printf("No Collision\n");
|
||||
else {
|
||||
std::printf("collision detected at normal: %f, %f, %f\n", simpletest.normalWorld.x, simpletest.normalWorld.y, simpletest.normalWorld.z);
|
||||
std::printf("collision point : %f, %f, %f\n", (simpletest.collisionPointWorld).x, (simpletest.collisionPointWorld).y, simpletest.collisionPointWorld.z);
|
||||
}
|
||||
// case 1 result:
|
||||
// collision detected at normal: -1.000000, -0.000000, -0.000000
|
||||
// collision point : 1.500000, 1.500000, 1.500000
|
||||
// Box A should be pushed to the left
|
||||
}
|
||||
else if (caseid == 2){// case 2, collide at a corner of Box B:
|
||||
GamePhysics::Mat4 AM, BM;
|
||||
AM.initTranslation(0.2f, 5.0f, 1.0f); // box A moves(0.2f, 5.0f, 1.0f) from origin
|
||||
BM.initRotationZ(45); // box B rotates 45 degree around axis z
|
||||
// box A size(9,2,3), box B size(5.656854f, 5.656854f, 2.0f)
|
||||
GamePhysics::Mat4 SizeMat;
|
||||
SizeMat.initScaling(9.0f, 2.0f, 3.0f);
|
||||
AM = SizeMat * AM;
|
||||
SizeMat.initScaling(5.656854f, 5.656854f, 2.0f);
|
||||
BM = SizeMat * BM;
|
||||
// check for collision
|
||||
CollisionInfo simpletest = checkCollisionSAT(AM, BM);// should find out a collision here
|
||||
|
||||
if (!simpletest.isValid)
|
||||
std::printf("No Collision\n");
|
||||
else {
|
||||
std::printf("collision detected at normal: %f, %f, %f\n", simpletest.normalWorld.x, simpletest.normalWorld.y, simpletest.normalWorld.z);
|
||||
std::printf("collision point : %f, %f, %f\n", (simpletest.collisionPointWorld).x, (simpletest.collisionPointWorld).y, simpletest.collisionPointWorld.z);
|
||||
}
|
||||
// case 2 result:
|
||||
// collision detected at normal : 0.000000, 1.000000, 0.000000
|
||||
// collision point : 0.000000, 4.000000, 1.000000
|
||||
}
|
||||
else if (caseid == 3){// case 3, collide at a corner of Box A:
|
||||
// box A first rotates 45 degree around axis z
|
||||
// box A moves(-2.0f, 0.0f, 1.0f) from origin,(-2.0f,0.0f,1.0f) is the centre position of A in world space
|
||||
// box A size(2.829f, 2.829f, 2.0f)
|
||||
GamePhysics::Mat4 AM_rot; AM_rot.initRotationZ(45);
|
||||
GamePhysics::Mat4 AM_tra; AM_tra.initTranslation(-2.0f, 0.0f, 1.0f);
|
||||
GamePhysics::Mat4 AM_sca; AM_sca.initScaling(2.829f, 2.829f, 2.0f);
|
||||
// get the object 2 world matrix of A
|
||||
GamePhysics::Mat4 AM = AM_sca * AM_rot * AM_tra; // pay attention to the order!
|
||||
// order, since we are working with the DirectX, we use left-handed matrixes!
|
||||
|
||||
// box B first rotates 90 degree around axis z
|
||||
// box B then moves (1.0f,0.5f,0.0f) from origin, (1.0f,0.5f,0.0f) is also the centre position of B in world space
|
||||
// box B size(9.0f, 2.0f, 4.0f)
|
||||
GamePhysics::Mat4 BM_rot; BM_rot.initRotationZ(90);
|
||||
GamePhysics::Mat4 BM_tra; BM_tra.initTranslation(1.0f, 0.5f, 0.0f);
|
||||
GamePhysics::Mat4 BM_sca; BM_sca.initScaling(9.0f, 2.0f, 4.0f);
|
||||
GamePhysics::Mat4 BM = BM_sca * BM_rot * BM_tra; // pay attention to the order!
|
||||
|
||||
// check for collision
|
||||
CollisionInfo simpletest = checkCollisionSAT(AM, BM);// should find out a collision here
|
||||
|
||||
if (!simpletest.isValid)
|
||||
std::printf("No Collision\n");
|
||||
else {
|
||||
std::printf("collision detected at normal: %f, %f, %f\n", simpletest.normalWorld.x, simpletest.normalWorld.y, simpletest.normalWorld.z);
|
||||
std::printf("collision point : %f, %f, %f\n", (simpletest.collisionPointWorld).x, (simpletest.collisionPointWorld).y, simpletest.collisionPointWorld.z);
|
||||
}
|
||||
// case 3 result:
|
||||
// collision detected at normal: -1.000000, 0.000000, -0.000000
|
||||
// collision point : 0.000405, 0.000000, 0.000000
|
||||
}
|
||||
}
|
||||
@@ -1,160 +0,0 @@
|
||||
/******************************************************************************
|
||||
*
|
||||
* MantaFlow fluid solver framework
|
||||
* Copyright 2011 Tobias Pfaff, Nils Thuerey
|
||||
*
|
||||
* This program is free software, distributed under the terms of the
|
||||
* GNU General Public License (GPL)
|
||||
* http://www.gnu.org/licenses
|
||||
*
|
||||
* Globally used macros and functions
|
||||
*
|
||||
******************************************************************************/
|
||||
|
||||
#ifndef _GENERAL_H
|
||||
#define _GENERAL_H
|
||||
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
|
||||
namespace Manta {
|
||||
|
||||
// ui data exchange
|
||||
#ifdef GUI
|
||||
// defined in qtmain.cpp
|
||||
extern void updateQtGui(bool full, int frame, float time, const std::string& curPlugin);
|
||||
#else
|
||||
// dummy function if GUI is not enabled
|
||||
inline void updateQtGui(bool full, int frame, float time, const std::string& curPlugin) {}
|
||||
#endif
|
||||
|
||||
|
||||
// activate debug mode if _DEBUG is defined (eg for windows)
|
||||
#ifndef DEBUG
|
||||
#ifdef _DEBUG
|
||||
#define DEBUG 1
|
||||
#endif // _DEBUG
|
||||
#endif // DEBUG
|
||||
|
||||
// Standard exception
|
||||
class Error : public std::exception
|
||||
{
|
||||
public:
|
||||
Error(const std::string& s) : mS(s) {
|
||||
# ifdef DEBUG
|
||||
// print error
|
||||
std::cerr << "Aborting: "<< s <<" \n";
|
||||
// then force immedieate crash in debug mode
|
||||
*(volatile int*)(0) = 1;
|
||||
# endif
|
||||
}
|
||||
virtual ~Error() throw() {}
|
||||
virtual const char* what() const throw() { return mS.c_str(); }
|
||||
private:
|
||||
std::string mS;
|
||||
};
|
||||
|
||||
// mark unused parameter variables
|
||||
#define unusedParameter(x) ((void)x)
|
||||
|
||||
// Debug output functions and macros
|
||||
extern int gDebugLevel;
|
||||
|
||||
#define MSGSTREAM std::ostringstream msg; msg.precision(7); msg.width(9);
|
||||
#define debMsg(mStr, level) if (_chklevel(level)) { MSGSTREAM; msg << mStr; std::cout << msg.str() << std::endl; }
|
||||
inline bool _chklevel(int level=0) { return gDebugLevel >= level; }
|
||||
|
||||
// error and assertation macros
|
||||
#ifdef DEBUG
|
||||
# define DEBUG_ONLY(a) a
|
||||
#else
|
||||
# define DEBUG_ONLY(a)
|
||||
#endif
|
||||
#define throwError(msg) { std::ostringstream __s; __s << msg << std::endl << "Error raised in " << __FILE__ << ":" << __LINE__; throw Manta::Error(__s.str()); }
|
||||
#define errMsg(msg) throwError(msg);
|
||||
#define assertMsg(cond,msg) if(!(cond)) throwError(msg)
|
||||
#define assertDeb(cond,msg) DEBUG_ONLY( assertMsg(cond,msg) )
|
||||
|
||||
// for compatibility with blender, blender only defines WITH_MANTA, make sure we have "BLENDER"
|
||||
#ifndef BLENDER
|
||||
#ifdef WITH_MANTA
|
||||
#define BLENDER 1
|
||||
#endif
|
||||
#endif
|
||||
|
||||
// common type for indexing large grids
|
||||
typedef long long IndexInt;
|
||||
|
||||
// template tricks
|
||||
template<typename T>
|
||||
struct remove_pointers {
|
||||
typedef T type;
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
struct remove_pointers<T*> {
|
||||
typedef T type;
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
struct remove_pointers<T&> {
|
||||
typedef T type;
|
||||
};
|
||||
|
||||
// Commonly used enums and types
|
||||
//! Timing class for preformance measuring
|
||||
struct MuTime {
|
||||
MuTime() { get(); }
|
||||
MuTime operator-(const MuTime& a) { MuTime b; b.time = time - a.time; return b; };
|
||||
MuTime operator+(const MuTime& a) { MuTime b; b.time = time + a.time; return b; };
|
||||
MuTime operator/(unsigned long a) { MuTime b; b.time = time / a; return b; };
|
||||
MuTime& operator+=(const MuTime& a) { time += a.time; return *this; }
|
||||
MuTime& operator-=(const MuTime& a) { time -= a.time; return *this; }
|
||||
MuTime& operator/=(unsigned long a) { time /= a; return *this; }
|
||||
std::string toString();
|
||||
|
||||
void clear() { time = 0; }
|
||||
void get();
|
||||
MuTime update();
|
||||
|
||||
unsigned long time;
|
||||
};
|
||||
std::ostream& operator<< (std::ostream& os, const MuTime& t);
|
||||
|
||||
//! generate a string with infos about the current mantaflow build
|
||||
std::string buildInfoString();
|
||||
|
||||
// Some commonly used math helpers
|
||||
template<class T> inline T square(T a) {
|
||||
return a*a;
|
||||
}
|
||||
template<class T> inline T cubed(T a) {
|
||||
return a*a*a;
|
||||
}
|
||||
|
||||
template<class T> inline T clamp(const T& val, const T& vmin, const T& vmax) {
|
||||
if (val < vmin) return vmin;
|
||||
if (val > vmax) return vmax;
|
||||
return val;
|
||||
}
|
||||
|
||||
template<class T> inline T nmod(const T& a, const T& b);
|
||||
template<> inline int nmod(const int& a, const int& b) { int c=a%b; return (c<0) ? (c+b) : c; }
|
||||
template<> inline float nmod(const float& a, const float& b) { float c=std::fmod(a,b); return (c<0) ? (c+b) : c; }
|
||||
template<> inline double nmod(const double& a, const double& b) { double c=std::fmod(a,b); return (c<0) ? (c+b) : c; }
|
||||
|
||||
template<class T> inline T safeDivide(const T& a, const T& b);
|
||||
template<> inline int safeDivide<int>(const int &a, const int& b) { return (b) ? (a/b) : a; }
|
||||
template<> inline float safeDivide<float>(const float &a, const float& b) { return (b) ? (a/b) : a; }
|
||||
template<> inline double safeDivide<double>(const double &a, const double& b) { return (b) ? (a/b) : a; }
|
||||
|
||||
inline bool c_isnan(float c) {
|
||||
volatile float d=c;
|
||||
return d != d;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
#endif
|
||||
@@ -20,11 +20,10 @@ using namespace GamePhysics;
|
||||
|
||||
//#define ADAPTIVESTEP
|
||||
|
||||
//#define TEMPLATE_DEMO
|
||||
#define TEMPLATE_DEMO
|
||||
//#define MASS_SPRING_SYSTEM
|
||||
//#define RIGID_BODY_SYSTEM
|
||||
//#define SPH_SYSTEM
|
||||
#define DIFFUSION_SYSTEM
|
||||
|
||||
#ifdef TEMPLATE_DEMO
|
||||
#include "TemplateSimulator.h"
|
||||
@@ -39,10 +38,6 @@ using namespace GamePhysics;
|
||||
//#include "SPHSystemSimulator.h"
|
||||
#endif
|
||||
|
||||
#ifdef DIFFUSION_SYSTEM
|
||||
#include "DiffusionSimulator.h"
|
||||
#endif
|
||||
|
||||
DrawingUtilitiesClass * g_pDUC;
|
||||
Simulator * g_pSimulator;
|
||||
float g_fTimestep = 0.001;
|
||||
@@ -374,9 +369,6 @@ int main(int argc, char* argv[])
|
||||
#endif
|
||||
#ifdef SPH_SYSTEM
|
||||
//g_pSimulator= new SPHSystemSimulator();
|
||||
#endif
|
||||
#ifdef DIFFUSION_SYSTEM
|
||||
g_pSimulator= new DiffusionSimulator();
|
||||
#endif
|
||||
g_pSimulator->reset();
|
||||
|
||||
|
||||
@@ -1,750 +0,0 @@
|
||||
//
|
||||
// Preconditioned conjugate gradient solver
|
||||
//
|
||||
// Created by Robert Bridson, Ryoichi Ando and Nils Thuerey
|
||||
//
|
||||
|
||||
#ifndef RCMATRIX3_H
|
||||
#define RCMATRIX3_H
|
||||
|
||||
#include <iterator>
|
||||
#include <cassert>
|
||||
#include <vector>
|
||||
#include <fstream>
|
||||
#include <cmath>
|
||||
#include <functional>
|
||||
|
||||
// index type
|
||||
#define int_index long long
|
||||
|
||||
// parallelization disabled
|
||||
|
||||
#define parallel_for(size) { int thread_number = 0; int_index parallel_index=0; for( int_index parallel_index=0; parallel_index<(int_index)size; parallel_index++ ) {
|
||||
#define parallel_end } thread_number=parallel_index=0; }
|
||||
|
||||
#define parallel_block
|
||||
#define do_parallel
|
||||
#define do_end
|
||||
#define block_end
|
||||
|
||||
#include "vectorbase.h"
|
||||
|
||||
// note - "Int" instead of "N" here, the latter is size!
|
||||
template<class Int, class T>
|
||||
struct InstantBLAS {
|
||||
static inline Int offset(Int N, Int incX) { return ((incX) > 0 ? 0 : ((N) - 1) * (-(incX))); }
|
||||
static T cblas_ddot( const Int N, const T *X, const Int incX, const T *Y, const Int incY) {
|
||||
double r = 0.0; // always use double precision internally here...
|
||||
Int i;
|
||||
Int ix = offset(N,incX);
|
||||
Int iy = offset(N,incY);
|
||||
for (i = 0; i < N; i++) {
|
||||
r += X[ix] * Y[iy];
|
||||
ix += incX;
|
||||
iy += incY;
|
||||
}
|
||||
return (T)r;
|
||||
}
|
||||
static void cblas_daxpy( const Int N, const T alpha, const T *X, const Int incX, T *Y, const Int incY) {
|
||||
Int i;
|
||||
if (N <= 0 ) return;
|
||||
if (alpha == 0.0) return;
|
||||
if (incX == 1 && incY == 1) {
|
||||
const Int m = N % 4;
|
||||
for (i = 0; i < m; i++)
|
||||
Y[i] += alpha * X[i];
|
||||
for (i = m; i + 3 < N; i += 4) {
|
||||
Y[i ] += alpha * X[i ];
|
||||
Y[i + 1] += alpha * X[i + 1];
|
||||
Y[i + 2] += alpha * X[i + 2];
|
||||
Y[i + 3] += alpha * X[i + 3];
|
||||
}
|
||||
} else {
|
||||
Int ix = offset(N, incX);
|
||||
Int iy = offset(N, incY);
|
||||
for (i = 0; i < N; i++) {
|
||||
Y[iy] += alpha * X[ix];
|
||||
ix += incX;
|
||||
iy += incY;
|
||||
}
|
||||
}
|
||||
}
|
||||
// dot products ==============================================================
|
||||
static inline T dot(const std::vector<T> &x, const std::vector<T> &y) {
|
||||
return cblas_ddot((int)x.size(), &x[0], 1, &y[0], 1);
|
||||
}
|
||||
|
||||
// inf-norm (maximum absolute value: index of max returned) ==================
|
||||
static inline Int index_abs_max(const std::vector<T> &x) {
|
||||
int maxind = 0;
|
||||
T maxvalue = 0;
|
||||
for(Int i = 0; i < (Int)x.size(); ++i) {
|
||||
if(std::abs(x[i]) > maxvalue) {
|
||||
maxvalue = fabs(x[i]);
|
||||
maxind = i;
|
||||
}
|
||||
}
|
||||
return maxind;
|
||||
}
|
||||
|
||||
// inf-norm (maximum absolute value) =========================================
|
||||
// technically not part of BLAS, but useful
|
||||
static inline T abs_max(const std::vector<T> &x)
|
||||
{ return std::abs(x[index_abs_max(x)]); }
|
||||
|
||||
// saxpy (y=alpha*x+y) =======================================================
|
||||
static inline void add_scaled(T alpha, const std::vector<T> &x, std::vector<T> &y) {
|
||||
cblas_daxpy((Int)x.size(), alpha, &x[0], 1, &y[0], 1);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
template<class T>
|
||||
void zero(std::vector<T> &v)
|
||||
{ for(int i=(int)v.size()-1; i>=0; --i) v[i]=0; }
|
||||
|
||||
template<class T>
|
||||
void insert(std::vector<T> &a, unsigned int index, T e)
|
||||
{
|
||||
a.push_back(a.back());
|
||||
for(unsigned int i=(unsigned int)a.size()-1; i>index; --i)
|
||||
a[i]=a[i-1];
|
||||
a[index]=e;
|
||||
}
|
||||
|
||||
template<class T>
|
||||
void erase(std::vector<T> &a, unsigned int index)
|
||||
{
|
||||
for(unsigned int i=index; i<a.size()-1; ++i)
|
||||
a[i]=a[i+1];
|
||||
a.pop_back();
|
||||
}
|
||||
|
||||
//============================================================================
|
||||
// Dynamic compressed sparse row matrix.
|
||||
|
||||
template<class T>
|
||||
struct SparseMatrix
|
||||
{
|
||||
int n; // dimension
|
||||
std::vector<std::vector<int> > index; // for each row, a list of all column indices (sorted)
|
||||
std::vector<std::vector<T> > value; // values corresponding to index
|
||||
|
||||
explicit SparseMatrix(int n_=0, int expected_nonzeros_per_row=7)
|
||||
: n(n_), index(n_), value(n_)
|
||||
{
|
||||
for(int i=0; i<n; ++i){
|
||||
index[i].reserve(expected_nonzeros_per_row);
|
||||
value[i].reserve(expected_nonzeros_per_row);
|
||||
}
|
||||
}
|
||||
|
||||
void clear(void)
|
||||
{
|
||||
n=0;
|
||||
index.clear();
|
||||
value.clear();
|
||||
}
|
||||
|
||||
void zero(void)
|
||||
{
|
||||
for(int i=0; i<n; ++i){
|
||||
index[i].resize(0);
|
||||
value[i].resize(0);
|
||||
}
|
||||
}
|
||||
|
||||
void resize(int n_)
|
||||
{
|
||||
n=n_;
|
||||
index.resize(n);
|
||||
value.resize(n);
|
||||
}
|
||||
|
||||
T operator()(int i, int j) const
|
||||
{
|
||||
for(int k=0; k<(int)index[i].size(); ++k){
|
||||
if(index[i][k]==j) return value[i][k];
|
||||
else if(index[i][k]>j) return 0;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
void set_element(int i, int j, T new_value)
|
||||
{
|
||||
int k=0;
|
||||
for(; k<(int)index[i].size(); ++k){
|
||||
if(index[i][k]==j){
|
||||
value[i][k]=new_value;
|
||||
return;
|
||||
}else if(index[i][k]>j){
|
||||
insert(index[i], k, j);
|
||||
insert(value[i], k, new_value);
|
||||
return;
|
||||
}
|
||||
}
|
||||
index[i].push_back(j);
|
||||
value[i].push_back(new_value);
|
||||
}
|
||||
|
||||
void add_to_element(int i, int j, T increment_value)
|
||||
{
|
||||
int k=0;
|
||||
for(; k<(int)index[i].size(); ++k){
|
||||
if(index[i][k]==j){
|
||||
value[i][k]+=increment_value;
|
||||
return;
|
||||
}else if(index[i][k]>j){
|
||||
insert(index[i], k, j);
|
||||
insert(value[i], k, increment_value);
|
||||
return;
|
||||
}
|
||||
}
|
||||
index[i].push_back(j);
|
||||
value[i].push_back(increment_value);
|
||||
}
|
||||
|
||||
// assumes indices is already sorted
|
||||
void add_sparse_row(int i, const std::vector<int> &indices, const std::vector<T> &values)
|
||||
{
|
||||
int j=0, k=0;
|
||||
while(j<indices.size() && k<(int)index[i].size()){
|
||||
if(index[i][k]<indices[j]){
|
||||
++k;
|
||||
}else if(index[i][k]>indices[j]){
|
||||
insert(index[i], k, indices[j]);
|
||||
insert(value[i], k, values[j]);
|
||||
++j;
|
||||
}else{
|
||||
value[i][k]+=values[j];
|
||||
++j;
|
||||
++k;
|
||||
}
|
||||
}
|
||||
for(;j<indices.size(); ++j){
|
||||
index[i].push_back(indices[j]);
|
||||
value[i].push_back(values[j]);
|
||||
}
|
||||
}
|
||||
|
||||
// assumes matrix has symmetric structure - so the indices in row i tell us which columns to delete i from
|
||||
void symmetric_remove_row_and_column(int i)
|
||||
{
|
||||
for(int a=0; a<index[i].size(); ++a){
|
||||
int j=index[i][a]; //
|
||||
for(int b=0; b<index[j].size(); ++b){
|
||||
if(index[j][b]==i){
|
||||
erase(index[j], b);
|
||||
erase(value[j], b);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
index[i].resize(0);
|
||||
value[i].resize(0);
|
||||
}
|
||||
|
||||
void write_matlab(std::ostream &output, const char *variable_name)
|
||||
{
|
||||
output<<variable_name<<"=sparse([";
|
||||
for(int i=0; i<n; ++i){
|
||||
for(int j=0; j<index[i].size(); ++j){
|
||||
output<<i+1<<" ";
|
||||
}
|
||||
}
|
||||
output<<"],...\n [";
|
||||
for(int i=0; i<n; ++i){
|
||||
for(int j=0; j<index[i].size(); ++j){
|
||||
output<<index[i][j]+1<<" ";
|
||||
}
|
||||
}
|
||||
output<<"],...\n [";
|
||||
for(int i=0; i<n; ++i){
|
||||
for(int j=0; j<value[i].size(); ++j){
|
||||
output<<value[i][j]<<" ";
|
||||
}
|
||||
}
|
||||
output<<"], "<<n<<", "<<n<<");"<<std::endl;
|
||||
}
|
||||
};
|
||||
|
||||
typedef SparseMatrix<float> SparseMatrixf;
|
||||
typedef SparseMatrix<double> SparseMatrixd;
|
||||
|
||||
// perform result=matrix*x
|
||||
template<class T>
|
||||
void multiply(const SparseMatrix<T> &matrix, const std::vector<T> &x, std::vector<T> &result)
|
||||
{
|
||||
assert(matrix.n==x.size());
|
||||
result.resize(matrix.n);
|
||||
//for(int i=0; i<matrix.n; ++i)
|
||||
parallel_for(matrix.n) {
|
||||
unsigned i (parallel_index);
|
||||
T value=0;
|
||||
for(int j=0; j<(int)matrix.index[i].size(); ++j){
|
||||
value+=matrix.value[i][j]*x[matrix.index[i][j]];
|
||||
}
|
||||
result[i]=value;
|
||||
} parallel_end
|
||||
}
|
||||
|
||||
// perform result=result-matrix*x
|
||||
template<class T>
|
||||
void multiply_and_subtract(const SparseMatrix<T> &matrix, const std::vector<T> &x, std::vector<T> &result)
|
||||
{
|
||||
assert(matrix.n==x.size());
|
||||
result.resize(matrix.n);
|
||||
for(int i=0; i<(int)matrix.n; ++i){
|
||||
for(int j=0; j<(int)matrix.index[i].size(); ++j){
|
||||
result[i]-=matrix.value[i][j]*x[matrix.index[i][j]];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//============================================================================
|
||||
// Fixed version of SparseMatrix. This is not a good structure for dynamically
|
||||
// modifying the matrix, but can be significantly faster for matrix-vector
|
||||
// multiplies due to better data locality.
|
||||
|
||||
template<class T>
|
||||
struct FixedSparseMatrix
|
||||
{
|
||||
int n; // dimension
|
||||
std::vector<T> value; // nonzero values row by row
|
||||
std::vector<int> colindex; // corresponding column indices
|
||||
std::vector<int> rowstart; // where each row starts in value and colindex (and last entry is one past the end, the number of nonzeros)
|
||||
|
||||
explicit FixedSparseMatrix(int n_=0)
|
||||
: n(n_), value(0), colindex(0), rowstart(n_+1)
|
||||
{}
|
||||
|
||||
void clear(void)
|
||||
{
|
||||
n=0;
|
||||
value.clear();
|
||||
colindex.clear();
|
||||
rowstart.clear();
|
||||
}
|
||||
|
||||
void resize(int n_)
|
||||
{
|
||||
n=n_;
|
||||
rowstart.resize(n+1);
|
||||
}
|
||||
|
||||
void construct_from_matrix(const SparseMatrix<T> &matrix)
|
||||
{
|
||||
resize(matrix.n);
|
||||
rowstart[0]=0;
|
||||
for(int i=0; i<n; ++i){
|
||||
rowstart[i+1]=rowstart[i]+matrix.index[i].size();
|
||||
}
|
||||
value.resize(rowstart[n]);
|
||||
colindex.resize(rowstart[n]);
|
||||
int j=0;
|
||||
for(int i=0; i<n; ++i){
|
||||
for(int k=0; k<(int)matrix.index[i].size(); ++k){
|
||||
value[j]=matrix.value[i][k];
|
||||
colindex[j]=matrix.index[i][k];
|
||||
++j;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void write_matlab(std::ostream &output, const char *variable_name)
|
||||
{
|
||||
output<<variable_name<<"=sparse([";
|
||||
for(int i=0; i<n; ++i){
|
||||
for(int j=rowstart[i]; j<rowstart[i+1]; ++j){
|
||||
output<<i+1<<" ";
|
||||
}
|
||||
}
|
||||
output<<"],...\n [";
|
||||
for(int i=0; i<n; ++i){
|
||||
for(int j=rowstart[i]; j<rowstart[i+1]; ++j){
|
||||
output<<colindex[j]+1<<" ";
|
||||
}
|
||||
}
|
||||
output<<"],...\n [";
|
||||
for(int i=0; i<n; ++i){
|
||||
for(int j=rowstart[i]; j<rowstart[i+1]; ++j){
|
||||
output<<value[j]<<" ";
|
||||
}
|
||||
}
|
||||
output<<"], "<<n<<", "<<n<<");"<<std::endl;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
// perform result=matrix*x
|
||||
template<class T>
|
||||
void multiply(const FixedSparseMatrix<T> &matrix, const std::vector<T> &x, std::vector<T> &result)
|
||||
{
|
||||
assert(matrix.n==x.size());
|
||||
result.resize(matrix.n);
|
||||
//for(int i=0; i<matrix.n; ++i)
|
||||
parallel_for(matrix.n) {
|
||||
unsigned i (parallel_index);
|
||||
T value=0;
|
||||
for(int j=matrix.rowstart[i]; j<matrix.rowstart[i+1]; ++j){
|
||||
value+=matrix.value[j]*x[matrix.colindex[j]];
|
||||
}
|
||||
result[i]=value;
|
||||
} parallel_end
|
||||
}
|
||||
|
||||
// perform result=result-matrix*x
|
||||
template<class T>
|
||||
void multiply_and_subtract(const FixedSparseMatrix<T> &matrix, const std::vector<T> &x, std::vector<T> &result)
|
||||
{
|
||||
assert(matrix.n==x.size());
|
||||
result.resize(matrix.n);
|
||||
for(int i=0; i<matrix.n; ++i){
|
||||
for(int j=matrix.rowstart[i]; j<matrix.rowstart[i+1]; ++j){
|
||||
result[i]-=matrix.value[j]*x[matrix.colindex[j]];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//============================================================================
|
||||
// A simple compressed sparse column data structure (with separate diagonal)
|
||||
// for lower triangular matrices
|
||||
|
||||
template<class T>
|
||||
struct SparseColumnLowerFactor
|
||||
{
|
||||
int n;
|
||||
std::vector<T> invdiag; // reciprocals of diagonal elements
|
||||
std::vector<T> value; // values below the diagonal, listed column by column
|
||||
std::vector<int> rowindex; // a list of all row indices, for each column in turn
|
||||
std::vector<int> colstart; // where each column begins in rowindex (plus an extra entry at the end, of #nonzeros)
|
||||
std::vector<T> adiag; // just used in factorization: minimum "safe" diagonal entry allowed
|
||||
|
||||
explicit SparseColumnLowerFactor(int n_=0)
|
||||
: n(n_), invdiag(n_), colstart(n_+1), adiag(n_)
|
||||
{}
|
||||
|
||||
void clear(void)
|
||||
{
|
||||
n=0;
|
||||
invdiag.clear();
|
||||
value.clear();
|
||||
rowindex.clear();
|
||||
colstart.clear();
|
||||
adiag.clear();
|
||||
}
|
||||
|
||||
void resize(int n_)
|
||||
{
|
||||
n=n_;
|
||||
invdiag.resize(n);
|
||||
colstart.resize(n+1);
|
||||
adiag.resize(n);
|
||||
}
|
||||
|
||||
void write_matlab(std::ostream &output, const char *variable_name)
|
||||
{
|
||||
output<<variable_name<<"=sparse([";
|
||||
for(int i=0; i<n; ++i){
|
||||
output<<" "<<i+1;
|
||||
for(int j=colstart[i]; j<colstart[i+1]; ++j){
|
||||
output<<" "<<rowindex[j]+1;
|
||||
}
|
||||
}
|
||||
output<<"],...\n [";
|
||||
for(int i=0; i<n; ++i){
|
||||
output<<" "<<i+1;
|
||||
for(int j=colstart[i]; j<colstart[i+1]; ++j){
|
||||
output<<" "<<i+1;
|
||||
}
|
||||
}
|
||||
output<<"],...\n [";
|
||||
for(int i=0; i<n; ++i){
|
||||
output<<" "<<(invdiag[i]!=0 ? 1/invdiag[i] : 0);
|
||||
for(int j=colstart[i]; j<colstart[i+1]; ++j){
|
||||
output<<" "<<value[j];
|
||||
}
|
||||
}
|
||||
output<<"], "<<n<<", "<<n<<");"<<std::endl;
|
||||
}
|
||||
};
|
||||
|
||||
//============================================================================
|
||||
// Incomplete Cholesky factorization, level zero, with option for modified version.
|
||||
// Set modification_parameter between zero (regular incomplete Cholesky) and
|
||||
// one (fully modified version), with values close to one usually giving the best
|
||||
// results. The min_diagonal_ratio parameter is used to detect and correct
|
||||
// problems in factorization: if a pivot is this much less than the diagonal
|
||||
// entry from the original matrix, the original matrix entry is used instead.
|
||||
|
||||
template<class T>
|
||||
void factor_modified_incomplete_cholesky0(const SparseMatrix<T> &matrix, SparseColumnLowerFactor<T> &factor,
|
||||
T modification_parameter=0.97, T min_diagonal_ratio=0.25)
|
||||
{
|
||||
// first copy lower triangle of matrix into factor (Note: assuming A is symmetric of course!)
|
||||
factor.resize(matrix.n);
|
||||
zero(factor.invdiag); // important: eliminate old values from previous solves!
|
||||
factor.value.resize(0);
|
||||
factor.rowindex.resize(0);
|
||||
zero(factor.adiag);
|
||||
for(int i=0; i<matrix.n; ++i){
|
||||
factor.colstart[i]=(int)factor.rowindex.size();
|
||||
for(int j=0; j<(int)matrix.index[i].size(); ++j){
|
||||
if(matrix.index[i][j]>i){
|
||||
factor.rowindex.push_back(matrix.index[i][j]);
|
||||
factor.value.push_back(matrix.value[i][j]);
|
||||
}else if(matrix.index[i][j]==i){
|
||||
factor.invdiag[i]=factor.adiag[i]=matrix.value[i][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
factor.colstart[matrix.n]=(int)factor.rowindex.size();
|
||||
// now do the incomplete factorization (figure out numerical values)
|
||||
|
||||
// MATLAB code:
|
||||
// L=tril(A);
|
||||
// for k=1:size(L,2)
|
||||
// L(k,k)=sqrt(L(k,k));
|
||||
// L(k+1:end,k)=L(k+1:end,k)/L(k,k);
|
||||
// for j=find(L(:,k))'
|
||||
// if j>k
|
||||
// fullupdate=L(:,k)*L(j,k);
|
||||
// incompleteupdate=fullupdate.*(A(:,j)~=0);
|
||||
// missing=sum(fullupdate-incompleteupdate);
|
||||
// L(j:end,j)=L(j:end,j)-incompleteupdate(j:end);
|
||||
// L(j,j)=L(j,j)-omega*missing;
|
||||
// end
|
||||
// end
|
||||
// end
|
||||
|
||||
for(int k=0; k<matrix.n; ++k){
|
||||
if(factor.adiag[k]==0) continue; // null row/column
|
||||
// figure out the final L(k,k) entry
|
||||
if(factor.invdiag[k]<min_diagonal_ratio*factor.adiag[k])
|
||||
factor.invdiag[k]=1/sqrt(factor.adiag[k]); // drop to Gauss-Seidel here if the pivot looks dangerously small
|
||||
else
|
||||
factor.invdiag[k]=1/sqrt(factor.invdiag[k]);
|
||||
// finalize the k'th column L(:,k)
|
||||
for(int p=factor.colstart[k]; p<factor.colstart[k+1]; ++p){
|
||||
factor.value[p]*=factor.invdiag[k];
|
||||
}
|
||||
// incompletely eliminate L(:,k) from future columns, modifying diagonals
|
||||
for(int p=factor.colstart[k]; p<factor.colstart[k+1]; ++p){
|
||||
int j=factor.rowindex[p]; // work on column j
|
||||
T multiplier=factor.value[p];
|
||||
T missing=0;
|
||||
int a=factor.colstart[k];
|
||||
// first look for contributions to missing from dropped entries above the diagonal in column j
|
||||
int b=0;
|
||||
while(a<factor.colstart[k+1] && factor.rowindex[a]<j){
|
||||
// look for factor.rowindex[a] in matrix.index[j] starting at b
|
||||
while(b<(int)matrix.index[j].size()){
|
||||
if(matrix.index[j][b]<factor.rowindex[a])
|
||||
++b;
|
||||
else if(matrix.index[j][b]==factor.rowindex[a])
|
||||
break;
|
||||
else{
|
||||
missing+=factor.value[a];
|
||||
break;
|
||||
}
|
||||
}
|
||||
++a;
|
||||
}
|
||||
// adjust the diagonal j,j entry
|
||||
if(a<factor.colstart[k+1] && factor.rowindex[a]==j){
|
||||
factor.invdiag[j]-=multiplier*factor.value[a];
|
||||
}
|
||||
++a;
|
||||
// and now eliminate from the nonzero entries below the diagonal in column j (or add to missing if we can't)
|
||||
b=factor.colstart[j];
|
||||
while(a<factor.colstart[k+1] && b<factor.colstart[j+1]){
|
||||
if(factor.rowindex[b]<factor.rowindex[a])
|
||||
++b;
|
||||
else if(factor.rowindex[b]==factor.rowindex[a]){
|
||||
factor.value[b]-=multiplier*factor.value[a];
|
||||
++a;
|
||||
++b;
|
||||
}else{
|
||||
missing+=factor.value[a];
|
||||
++a;
|
||||
}
|
||||
}
|
||||
// and if there's anything left to do, add it to missing
|
||||
while(a<factor.colstart[k+1]){
|
||||
missing+=factor.value[a];
|
||||
++a;
|
||||
}
|
||||
// and do the final diagonal adjustment from the missing entries
|
||||
factor.invdiag[j]-=modification_parameter*multiplier*missing;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//============================================================================
|
||||
// Solution routines with lower triangular matrix.
|
||||
|
||||
// solve L*result=rhs
|
||||
template<class T>
|
||||
void solve_lower(const SparseColumnLowerFactor<T> &factor, const std::vector<T> &rhs, std::vector<T> &result)
|
||||
{
|
||||
assert(factor.n==rhs.size());
|
||||
assert(factor.n==result.size());
|
||||
result=rhs;
|
||||
for(int i=0; i<factor.n; ++i){
|
||||
result[i]*=factor.invdiag[i];
|
||||
for(int j=factor.colstart[i]; j<factor.colstart[i+1]; ++j){
|
||||
result[factor.rowindex[j]]-=factor.value[j]*result[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// solve L^T*result=rhs
|
||||
template<class T>
|
||||
void solve_lower_transpose_in_place(const SparseColumnLowerFactor<T> &factor, std::vector<T> &x)
|
||||
{
|
||||
assert(factor.n==(int)x.size());
|
||||
assert(factor.n>0);
|
||||
int i=factor.n;
|
||||
do{
|
||||
--i;
|
||||
for(int j=factor.colstart[i]; j<factor.colstart[i+1]; ++j){
|
||||
x[i]-=factor.value[j]*x[factor.rowindex[j]];
|
||||
}
|
||||
x[i]*=factor.invdiag[i];
|
||||
}while(i!=0);
|
||||
}
|
||||
|
||||
//============================================================================
|
||||
// Encapsulates the Conjugate Gradient algorithm with incomplete Cholesky
|
||||
// factorization preconditioner.
|
||||
|
||||
template <class T>
|
||||
struct SparsePCGSolver
|
||||
{
|
||||
SparsePCGSolver(void)
|
||||
{
|
||||
set_solver_parameters(1e-5, 100, 0.97, 0.25);
|
||||
}
|
||||
|
||||
void set_solver_parameters(T tolerance_factor_, int max_iterations_, T modified_incomplete_cholesky_parameter_=0.97, T min_diagonal_ratio_=0.25)
|
||||
{
|
||||
tolerance_factor=tolerance_factor_;
|
||||
if(tolerance_factor<1e-30) tolerance_factor=1e-30;
|
||||
max_iterations=max_iterations_;
|
||||
modified_incomplete_cholesky_parameter=modified_incomplete_cholesky_parameter_;
|
||||
min_diagonal_ratio=min_diagonal_ratio_;
|
||||
}
|
||||
|
||||
bool solve(const SparseMatrix<T> &matrix, const std::vector<T> &rhs, std::vector<T> &result, T &relative_residual_out, int &iterations_out, int precondition=2)
|
||||
{
|
||||
int n=matrix.n;
|
||||
if((int)m.size()!=n){ m.resize(n); s.resize(n); z.resize(n); r.resize(n); }
|
||||
zero(result);
|
||||
r=rhs;
|
||||
double residual_out=InstantBLAS<int,T>::abs_max(r);
|
||||
if(residual_out==0) {
|
||||
iterations_out=0;
|
||||
return true;
|
||||
}
|
||||
//double tol=tolerance_factor*residual_out; // relative residual
|
||||
double tol=tolerance_factor;
|
||||
double residual_0 = residual_out;
|
||||
|
||||
form_preconditioner(matrix, precondition);
|
||||
apply_preconditioner( r, z, precondition);
|
||||
double rho=InstantBLAS<int,T>::dot(z, r);
|
||||
if(rho==0 || rho!=rho) {
|
||||
iterations_out=0;
|
||||
return false;
|
||||
}
|
||||
|
||||
s=z;
|
||||
fixed_matrix.construct_from_matrix(matrix);
|
||||
int iteration;
|
||||
for(iteration=0; iteration<max_iterations; ++iteration){
|
||||
multiply(fixed_matrix, s, z);
|
||||
double alpha=rho/InstantBLAS<int,T>::dot(s, z);
|
||||
InstantBLAS<int,T>::add_scaled(alpha, s, result);
|
||||
InstantBLAS<int,T>::add_scaled(-alpha, z, r);
|
||||
residual_out=InstantBLAS<int,T>::abs_max(r);
|
||||
relative_residual_out = residual_out / residual_0;
|
||||
if(residual_out<=tol) {
|
||||
iterations_out=iteration+1;
|
||||
return true;
|
||||
}
|
||||
apply_preconditioner(r, z, precondition);
|
||||
double rho_new=InstantBLAS<int,T>::dot(z, r);
|
||||
double beta=rho_new/rho;
|
||||
InstantBLAS<int,T>::add_scaled(beta, s, z); s.swap(z); // s=beta*s+z
|
||||
rho=rho_new;
|
||||
}
|
||||
iterations_out=iteration;
|
||||
relative_residual_out = residual_out / residual_0;
|
||||
return false;
|
||||
}
|
||||
|
||||
protected:
|
||||
|
||||
// internal structures
|
||||
SparseColumnLowerFactor<T> ic_factor; // modified incomplete cholesky factor
|
||||
std::vector<T> m, z, s, r; // temporary vectors for PCG
|
||||
FixedSparseMatrix<T> fixed_matrix; // used within loop
|
||||
|
||||
// parameters
|
||||
T tolerance_factor;
|
||||
int max_iterations;
|
||||
T modified_incomplete_cholesky_parameter;
|
||||
T min_diagonal_ratio;
|
||||
|
||||
void form_preconditioner(const SparseMatrix<T>& matrix, int precondition=2)
|
||||
{
|
||||
if(precondition==2) {
|
||||
// incomplete cholesky
|
||||
factor_modified_incomplete_cholesky0(matrix, ic_factor, modified_incomplete_cholesky_parameter, min_diagonal_ratio);
|
||||
|
||||
} else if(precondition==1) {
|
||||
// diagonal
|
||||
ic_factor.resize(matrix.n);
|
||||
zero(ic_factor.invdiag);
|
||||
for(int i=0; i<matrix.n; ++i) {
|
||||
for(int j=0; j<(int)matrix.index[i].size(); ++j){
|
||||
if(matrix.index[i][j]==i){
|
||||
ic_factor.invdiag[i] = 1./matrix.value[i][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void apply_preconditioner(const std::vector<T> &x, std::vector<T> &result, int precondition=2)
|
||||
{
|
||||
if (precondition==2) {
|
||||
// incomplete cholesky
|
||||
solve_lower(ic_factor, x, result);
|
||||
solve_lower_transpose_in_place(ic_factor,result);
|
||||
} else if(precondition==1) {
|
||||
// diagonal
|
||||
for(int_index i=0; i<(int_index)result.size(); ++i) {
|
||||
result[i] = x[i] * ic_factor.invdiag[i];
|
||||
}
|
||||
} else {
|
||||
// off
|
||||
result = x;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
#undef parallel_for
|
||||
#undef parallel_end
|
||||
#undef int_index
|
||||
|
||||
#undef parallel_block
|
||||
#undef do_parallel
|
||||
#undef do_end
|
||||
#undef block_end
|
||||
|
||||
#endif
|
||||
@@ -646,8 +646,8 @@ matrix4x4<Scalar>::initRotationX(Scalar rot)
|
||||
|
||||
this->initId();
|
||||
value[1][1] = (Scalar) cos(drot);
|
||||
value[2][1] = (Scalar) sin(drot);
|
||||
value[1][2] = (Scalar)(-sin(drot));
|
||||
value[1][2] = (Scalar) sin(drot);
|
||||
value[2][1] = (Scalar)(-sin(drot));
|
||||
value[2][2] = (Scalar) cos(drot);
|
||||
}
|
||||
template<class Scalar>
|
||||
@@ -659,8 +659,8 @@ matrix4x4<Scalar>::initRotationY(Scalar rot)
|
||||
|
||||
this->initId();
|
||||
value[0][0] = (Scalar) cos(drot);
|
||||
value[2][0] = (Scalar)(-sin(drot));
|
||||
value[0][2] = (Scalar) sin(drot);
|
||||
value[0][2] = (Scalar)(-sin(drot));
|
||||
value[2][0] = (Scalar) sin(drot);
|
||||
value[2][2] = (Scalar) cos(drot);
|
||||
}
|
||||
template<class Scalar>
|
||||
@@ -672,8 +672,8 @@ matrix4x4<Scalar>::initRotationZ(Scalar rot)
|
||||
|
||||
this->initId();
|
||||
value[0][0] = (Scalar) cos(drot);
|
||||
value[1][0] = (Scalar) sin(drot);
|
||||
value[0][1] = (Scalar)(-sin(drot));
|
||||
value[0][1] = (Scalar) sin(drot);
|
||||
value[1][0] = (Scalar)(-sin(drot));
|
||||
value[1][1] = (Scalar) cos(drot);
|
||||
}
|
||||
template<class Scalar>
|
||||
|
||||
@@ -14,29 +14,29 @@ namespace GamePhysics
|
||||
|
||||
// basic inlined vector class
|
||||
template<class Scalar>
|
||||
class vector4Dim
|
||||
class ntlVector4Dim
|
||||
{
|
||||
public:
|
||||
//! Constructor
|
||||
inline vector4Dim() : x(0),y(0),z(0),t(0) {}
|
||||
inline ntlVector4Dim() : x(0),y(0),z(0),t(0) {}
|
||||
|
||||
//! Copy-Constructor
|
||||
inline vector4Dim ( const vector4Dim<Scalar> &v ) : x(v.x), y(v.y), z(v.z),t(v.t) {}
|
||||
inline ntlVector4Dim ( const ntlVector4Dim<Scalar> &v ) : x(v.x), y(v.y), z(v.z),t(v.t) {}
|
||||
|
||||
//! Copy-Constructor
|
||||
inline vector4Dim ( const float * v) : x((Scalar)v[0]), y((Scalar)v[1]), z((Scalar)v[2]), t((Scalar)v[3]) {}
|
||||
inline ntlVector4Dim ( const float * v) : x((Scalar)v[0]), y((Scalar)v[1]), z((Scalar)v[2]), t((Scalar)v[3]) {}
|
||||
|
||||
//! Copy-Constructor
|
||||
inline vector4Dim ( const double * v) : x((Scalar)v[0]), y((Scalar)v[1]), z((Scalar)v[2]), t((Scalar)v[3]) {}
|
||||
inline ntlVector4Dim ( const double * v) : x((Scalar)v[0]), y((Scalar)v[1]), z((Scalar)v[2]), t((Scalar)v[3]) {}
|
||||
|
||||
//! Construct a vector from one Scalar
|
||||
inline vector4Dim ( Scalar v) : x(v), y(v), z(v), t(v) {}
|
||||
inline ntlVector4Dim ( Scalar v) : x(v), y(v), z(v), t(v) {}
|
||||
|
||||
//! Construct a vector from four Ss
|
||||
inline vector4Dim ( Scalar vx, Scalar vy, Scalar vz, Scalar vw) : x(vx), y(vy), z(vz), t(vw) {}
|
||||
inline ntlVector4Dim ( Scalar vx, Scalar vy, Scalar vz, Scalar vw) : x(vx), y(vy), z(vz), t(vw) {}
|
||||
|
||||
//! Construct a vector from four Ss
|
||||
//inline vector4Dim(DirectX::XMVECTOR &v ); // TODO CHECK!
|
||||
//inline ntlVector4Dim(DirectX::XMVECTOR &v ); // TODO CHECK!
|
||||
|
||||
// get address of array for OpenGL
|
||||
Scalar *getAddress() { return value; }
|
||||
@@ -44,7 +44,7 @@ public:
|
||||
// Operators
|
||||
|
||||
//! Assignment operator
|
||||
inline const vector4Dim<Scalar>& operator= ( const vector4Dim<Scalar>& v ) {
|
||||
inline const ntlVector4Dim<Scalar>& operator= ( const ntlVector4Dim<Scalar>& v ) {
|
||||
x = v.x;
|
||||
y = v.y;
|
||||
z = v.z;
|
||||
@@ -52,12 +52,12 @@ public:
|
||||
return *this;
|
||||
}
|
||||
//! Assignment operator
|
||||
inline const vector4Dim<Scalar>& operator= ( Scalar s ) {
|
||||
inline const ntlVector4Dim<Scalar>& operator= ( Scalar s ) {
|
||||
x = y = z = t = s;
|
||||
return *this;
|
||||
}
|
||||
//! Assign and add operator
|
||||
inline const vector4Dim<Scalar>& operator+= ( const vector4Dim<Scalar>& v ) {
|
||||
inline const ntlVector4Dim<Scalar>& operator+= ( const ntlVector4Dim<Scalar>& v ) {
|
||||
x += v.x;
|
||||
y += v.y;
|
||||
z += v.z;
|
||||
@@ -65,7 +65,7 @@ public:
|
||||
return *this;
|
||||
}
|
||||
//! Assign and add operator
|
||||
inline const vector4Dim<Scalar>& operator+= ( Scalar s ) {
|
||||
inline const ntlVector4Dim<Scalar>& operator+= ( Scalar s ) {
|
||||
x += s;
|
||||
y += s;
|
||||
z += s;
|
||||
@@ -73,7 +73,7 @@ public:
|
||||
return *this;
|
||||
}
|
||||
//! Assign and sub operator
|
||||
inline const vector4Dim<Scalar>& operator-= ( const vector4Dim<Scalar>& v ) {
|
||||
inline const ntlVector4Dim<Scalar>& operator-= ( const ntlVector4Dim<Scalar>& v ) {
|
||||
x -= v.x;
|
||||
y -= v.y;
|
||||
z -= v.z;
|
||||
@@ -81,7 +81,7 @@ public:
|
||||
return *this;
|
||||
}
|
||||
//! Assign and sub operator
|
||||
inline const vector4Dim<Scalar>& operator-= ( Scalar s ) {
|
||||
inline const ntlVector4Dim<Scalar>& operator-= ( Scalar s ) {
|
||||
x -= s;
|
||||
y -= s;
|
||||
z -= s;
|
||||
@@ -89,7 +89,7 @@ public:
|
||||
return *this;
|
||||
}
|
||||
//! Assign and mult operator
|
||||
inline const vector4Dim<Scalar>& operator*= ( const vector4Dim<Scalar>& v ) {
|
||||
inline const ntlVector4Dim<Scalar>& operator*= ( const ntlVector4Dim<Scalar>& v ) {
|
||||
x *= v.x;
|
||||
y *= v.y;
|
||||
z *= v.z;
|
||||
@@ -97,7 +97,7 @@ public:
|
||||
return *this;
|
||||
}
|
||||
//! Assign and mult operator
|
||||
inline const vector4Dim<Scalar>& operator*= ( Scalar s ) {
|
||||
inline const ntlVector4Dim<Scalar>& operator*= ( Scalar s ) {
|
||||
x *= s;
|
||||
y *= s;
|
||||
z *= s;
|
||||
@@ -105,7 +105,7 @@ public:
|
||||
return *this;
|
||||
}
|
||||
//! Assign and div operator
|
||||
inline const vector4Dim<Scalar>& operator/= ( const vector4Dim<Scalar>& v ) {
|
||||
inline const ntlVector4Dim<Scalar>& operator/= ( const ntlVector4Dim<Scalar>& v ) {
|
||||
x /= v.x;
|
||||
y /= v.y;
|
||||
z /= v.z;
|
||||
@@ -113,7 +113,7 @@ public:
|
||||
return *this;
|
||||
}
|
||||
//! Assign and div operator
|
||||
inline const vector4Dim<Scalar>& operator/= ( Scalar s ) {
|
||||
inline const ntlVector4Dim<Scalar>& operator/= ( Scalar s ) {
|
||||
x /= s;
|
||||
y /= s;
|
||||
z /= s;
|
||||
@@ -121,29 +121,29 @@ public:
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline void safeDivide (const vector4Dim<Scalar>& v);
|
||||
inline void safeDivide (const ntlVector4Dim<Scalar>& v);
|
||||
|
||||
//! Negation operator
|
||||
inline vector4Dim<Scalar> operator- () const {
|
||||
return vector4Dim<Scalar> (-x, -y, -z, -t);
|
||||
inline ntlVector4Dim<Scalar> operator- () const {
|
||||
return ntlVector4Dim<Scalar> (-x, -y, -z, -t);
|
||||
}
|
||||
|
||||
// binary operator add
|
||||
inline vector4Dim<Scalar> operator+ (const vector4Dim<Scalar>&) const;
|
||||
inline ntlVector4Dim<Scalar> operator+ (const ntlVector4Dim<Scalar>&) const;
|
||||
// binary operator add
|
||||
inline vector4Dim<Scalar> operator+ (Scalar) const;
|
||||
inline ntlVector4Dim<Scalar> operator+ (Scalar) const;
|
||||
// binary operator sub
|
||||
inline vector4Dim<Scalar> operator- (const vector4Dim<Scalar>&) const;
|
||||
inline ntlVector4Dim<Scalar> operator- (const ntlVector4Dim<Scalar>&) const;
|
||||
// binary operator sub
|
||||
inline vector4Dim<Scalar> operator- (Scalar) const;
|
||||
inline ntlVector4Dim<Scalar> operator- (Scalar) const;
|
||||
// binary operator mult
|
||||
inline vector4Dim<Scalar> operator* (const vector4Dim<Scalar>&) const;
|
||||
inline ntlVector4Dim<Scalar> operator* (const ntlVector4Dim<Scalar>&) const;
|
||||
// binary operator mult
|
||||
inline vector4Dim<Scalar> operator* (Scalar) const;
|
||||
inline ntlVector4Dim<Scalar> operator* (Scalar) const;
|
||||
// binary operator div
|
||||
inline vector4Dim<Scalar> operator/ (const vector4Dim<Scalar>&) const;
|
||||
inline ntlVector4Dim<Scalar> operator/ (const ntlVector4Dim<Scalar>&) const;
|
||||
// binary operator div
|
||||
inline vector4Dim<Scalar> operator/ (Scalar) const;
|
||||
inline ntlVector4Dim<Scalar> operator/ (Scalar) const;
|
||||
|
||||
//! Get smallest component
|
||||
//inline Scalar min() const { return ( x<y ) ? ( ( x<z ) ? x:z ) : ( ( y<z ) ? y:z ); // todo t!!}
|
||||
@@ -185,7 +185,7 @@ public:
|
||||
};
|
||||
|
||||
// zero element
|
||||
static const vector4Dim<Scalar> ZERO;
|
||||
static const ntlVector4Dim<Scalar> ZERO;
|
||||
|
||||
protected:
|
||||
|
||||
@@ -196,9 +196,9 @@ protected:
|
||||
//************************************************************************
|
||||
//! Addition operator
|
||||
template<class Scalar>
|
||||
inline vector4Dim<Scalar> vector4Dim<Scalar>::operator+ ( const vector4Dim<Scalar> &v) const
|
||||
inline ntlVector4Dim<Scalar> ntlVector4Dim<Scalar>::operator+ ( const ntlVector4Dim<Scalar> &v) const
|
||||
{
|
||||
return vector4Dim<Scalar> (value[0]+v.value[0],
|
||||
return ntlVector4Dim<Scalar> (value[0]+v.value[0],
|
||||
value[1]+v.value[1],
|
||||
value[2]+v.value[2],
|
||||
value[3]+v.value[3]);
|
||||
@@ -206,42 +206,42 @@ inline vector4Dim<Scalar> vector4Dim<Scalar>::operator+ ( const vector4Dim<Scala
|
||||
|
||||
//! Addition operator
|
||||
template<class Scalar>
|
||||
inline vector4Dim<Scalar>
|
||||
vector4Dim<Scalar>::operator+(Scalar s) const
|
||||
inline ntlVector4Dim<Scalar>
|
||||
ntlVector4Dim<Scalar>::operator+(Scalar s) const
|
||||
{
|
||||
return vector4Dim<Scalar>(value[0]+s,
|
||||
return ntlVector4Dim<Scalar>(value[0]+s,
|
||||
value[1]+s,
|
||||
value[2]+s,
|
||||
value[3]+s);
|
||||
}
|
||||
|
||||
template<class Scalar>
|
||||
inline vector4Dim<Scalar>
|
||||
operator+(float s, vector4Dim<Scalar> v)
|
||||
inline ntlVector4Dim<Scalar>
|
||||
operator+(float s, ntlVector4Dim<Scalar> v)
|
||||
{
|
||||
return v + s;
|
||||
}
|
||||
|
||||
template<class Scalar>
|
||||
inline vector4Dim<Scalar>
|
||||
operator+(double s, vector4Dim<Scalar> v)
|
||||
inline ntlVector4Dim<Scalar>
|
||||
operator+(double s, ntlVector4Dim<Scalar> v)
|
||||
{
|
||||
return v + s;
|
||||
}
|
||||
|
||||
template<class Scalar>
|
||||
inline vector4Dim<Scalar>
|
||||
operator+(int s, vector4Dim<Scalar> v)
|
||||
inline ntlVector4Dim<Scalar>
|
||||
operator+(int s, ntlVector4Dim<Scalar> v)
|
||||
{
|
||||
return v + s;
|
||||
}
|
||||
|
||||
//! Subtraction operator
|
||||
template<class Scalar>
|
||||
inline vector4Dim<Scalar>
|
||||
vector4Dim<Scalar>::operator-( const vector4Dim<Scalar> &v ) const
|
||||
inline ntlVector4Dim<Scalar>
|
||||
ntlVector4Dim<Scalar>::operator-( const ntlVector4Dim<Scalar> &v ) const
|
||||
{
|
||||
return vector4Dim<Scalar>(value[0]-v.value[0],
|
||||
return ntlVector4Dim<Scalar>(value[0]-v.value[0],
|
||||
value[1]-v.value[1],
|
||||
value[2]-v.value[2],
|
||||
value[3]-v.value[3]);
|
||||
@@ -249,10 +249,10 @@ vector4Dim<Scalar>::operator-( const vector4Dim<Scalar> &v ) const
|
||||
|
||||
//! Subtraction operator
|
||||
template<class Scalar>
|
||||
inline vector4Dim<Scalar>
|
||||
vector4Dim<Scalar>::operator-(Scalar s ) const
|
||||
inline ntlVector4Dim<Scalar>
|
||||
ntlVector4Dim<Scalar>::operator-(Scalar s ) const
|
||||
{
|
||||
return vector4Dim<Scalar>(value[0]-s,
|
||||
return ntlVector4Dim<Scalar>(value[0]-s,
|
||||
value[1]-s,
|
||||
value[2]-s,
|
||||
value[3]-s,);
|
||||
@@ -260,50 +260,50 @@ vector4Dim<Scalar>::operator-(Scalar s ) const
|
||||
|
||||
//! Multiplication operator
|
||||
template<class Scalar>
|
||||
inline vector4Dim<Scalar>
|
||||
vector4Dim<Scalar>::operator* ( const vector4Dim<Scalar>& v ) const
|
||||
inline ntlVector4Dim<Scalar>
|
||||
ntlVector4Dim<Scalar>::operator* ( const ntlVector4Dim<Scalar>& v ) const
|
||||
{
|
||||
return vector4Dim<Scalar>(value[0]*v.value[0],
|
||||
return ntlVector4Dim<Scalar>(value[0]*v.value[0],
|
||||
value[1]*v.value[1],
|
||||
value[2]*v.value[2],
|
||||
value[3]*v.value[3]);
|
||||
}
|
||||
//! Multiplication operator
|
||||
template<class Scalar>
|
||||
inline vector4Dim<Scalar>
|
||||
vector4Dim<Scalar>::operator* (Scalar s) const
|
||||
inline ntlVector4Dim<Scalar>
|
||||
ntlVector4Dim<Scalar>::operator* (Scalar s) const
|
||||
{
|
||||
return vector4Dim<Scalar>(value[0]*s, value[1]*s, value[2]*s, value[3]*s);
|
||||
return ntlVector4Dim<Scalar>(value[0]*s, value[1]*s, value[2]*s, value[3]*s);
|
||||
}
|
||||
|
||||
//! Multiplication operator
|
||||
template<class Scalar>
|
||||
inline vector4Dim<Scalar>
|
||||
operator* (float s, vector4Dim<Scalar> v)
|
||||
inline ntlVector4Dim<Scalar>
|
||||
operator* (float s, ntlVector4Dim<Scalar> v)
|
||||
{
|
||||
return v * s;
|
||||
}
|
||||
|
||||
template<class Scalar>
|
||||
inline vector4Dim<Scalar>
|
||||
operator*(double s, vector4Dim<Scalar> v)
|
||||
inline ntlVector4Dim<Scalar>
|
||||
operator*(double s, ntlVector4Dim<Scalar> v)
|
||||
{
|
||||
return v * s;
|
||||
}
|
||||
|
||||
template<class Scalar>
|
||||
inline vector4Dim<Scalar>
|
||||
operator*(int s, vector4Dim<Scalar> v)
|
||||
inline ntlVector4Dim<Scalar>
|
||||
operator*(int s, ntlVector4Dim<Scalar> v)
|
||||
{
|
||||
return v * s;
|
||||
}
|
||||
|
||||
//! Division operator
|
||||
template<class Scalar>
|
||||
inline vector4Dim<Scalar>
|
||||
vector4Dim<Scalar>::operator/ (const vector4Dim<Scalar> & v) const
|
||||
inline ntlVector4Dim<Scalar>
|
||||
ntlVector4Dim<Scalar>::operator/ (const ntlVector4Dim<Scalar> & v) const
|
||||
{
|
||||
return vector4Dim<Scalar> (value[0]/v.value[0],
|
||||
return ntlVector4Dim<Scalar> (value[0]/v.value[0],
|
||||
value[1]/v.value[1],
|
||||
value[2]/v.value[2],
|
||||
value[3]/v.value[3]);
|
||||
@@ -311,10 +311,10 @@ vector4Dim<Scalar>::operator/ (const vector4Dim<Scalar> & v) const
|
||||
|
||||
//! Division operator
|
||||
template<class Scalar>
|
||||
inline vector4Dim<Scalar>
|
||||
vector4Dim<Scalar>::operator / (Scalar s) const
|
||||
inline ntlVector4Dim<Scalar>
|
||||
ntlVector4Dim<Scalar>::operator / (Scalar s) const
|
||||
{
|
||||
return vector4Dim<Scalar> (value[0]/s,
|
||||
return ntlVector4Dim<Scalar> (value[0]/s,
|
||||
value[1]/s,
|
||||
value[2]/s,
|
||||
value[3]/s);
|
||||
@@ -322,7 +322,7 @@ vector4Dim<Scalar>::operator / (Scalar s) const
|
||||
|
||||
//! Safe divide
|
||||
template<class Scalar>
|
||||
inline void vector4Dim<Scalar>::safeDivide( const vector4Dim<Scalar> &v )
|
||||
inline void ntlVector4Dim<Scalar>::safeDivide( const ntlVector4Dim<Scalar> &v )
|
||||
{
|
||||
value[0] = (v.value[0]!=0) ? (value[0] / v.value[0]) : 0;
|
||||
value[1] = (v.value[1]!=0) ? (value[1] / v.value[1]) : 0;
|
||||
@@ -336,14 +336,14 @@ inline void vector4Dim<Scalar>::safeDivide( const vector4Dim<Scalar> &v )
|
||||
|
||||
//! Dot product
|
||||
template<class Scalar>
|
||||
inline Scalar dot ( const vector4Dim<Scalar> &t, const vector4Dim<Scalar> &v ) {
|
||||
inline Scalar dot ( const ntlVector4Dim<Scalar> &t, const ntlVector4Dim<Scalar> &v ) {
|
||||
return t.x*v.x + t.y*v.y + t.z*v.z + t.t*v.t;
|
||||
}
|
||||
|
||||
//! Cross product
|
||||
/*template<class Scalar>
|
||||
inline vector4Dim<Scalar> cross ( const vector4Dim<Scalar> &t, const vector4Dim<Scalar> &v ) {
|
||||
NYI vector4Dim<Scalar> cp (
|
||||
inline ntlVector4Dim<Scalar> cross ( const ntlVector4Dim<Scalar> &t, const ntlVector4Dim<Scalar> &v ) {
|
||||
NYI ntlVector4Dim<Scalar> cp (
|
||||
( ( t.y*v.z ) - ( t.z*v.y ) ),
|
||||
( ( t.z*v.x ) - ( t.x*v.z ) ),
|
||||
( ( t.x*v.y ) - ( t.y*v.x ) ) );
|
||||
@@ -353,36 +353,36 @@ inline vector4Dim<Scalar> cross ( const vector4Dim<Scalar> &t, const vector4Dim<
|
||||
|
||||
//! Compute the magnitude (length) of the vector
|
||||
template<class Scalar>
|
||||
inline Scalar norm ( const vector4Dim<Scalar>& v ) {
|
||||
inline Scalar norm ( const ntlVector4Dim<Scalar>& v ) {
|
||||
Scalar l = v.x*v.x + v.y*v.y + v.z*v.z + v.t*v.t;
|
||||
return ( fabs ( l-1. ) < VECTOR_EPSILON*VECTOR_EPSILON ) ? 1. : sqrt ( l );
|
||||
}
|
||||
|
||||
//! Compute squared magnitude
|
||||
template<class Scalar>
|
||||
inline Scalar normSquare ( const vector4Dim<Scalar>& v ) {
|
||||
inline Scalar normSquare ( const ntlVector4Dim<Scalar>& v ) {
|
||||
return v.x*v.x + v.y*v.y + v.z*v.z + v.t*v.t;
|
||||
}
|
||||
|
||||
//! Returns a normalized vector
|
||||
template<class Scalar>
|
||||
inline vector4Dim<Scalar> getNormalized ( const vector4Dim<Scalar>& v ) {
|
||||
inline ntlVector4Dim<Scalar> getNormalized ( const ntlVector4Dim<Scalar>& v ) {
|
||||
Scalar l = v.x*v.x + v.y*v.y + v.z*v.z + v.t*v.t;
|
||||
if ( fabs ( l-1. ) < VECTOR_EPSILON*VECTOR_EPSILON )
|
||||
return v; /* normalized "enough"... */
|
||||
else if ( l > VECTOR_EPSILON*VECTOR_EPSILON )
|
||||
{
|
||||
Scalar fac = 1./sqrt ( l );
|
||||
return vector4Dim<Scalar> ( v.x*fac, v.y*fac, v.z*fac , v.t*fac );
|
||||
return ntlVector4Dim<Scalar> ( v.x*fac, v.y*fac, v.z*fac , v.t*fac );
|
||||
}
|
||||
else
|
||||
return vector4Dim<Scalar> ( ( Scalar ) 0 );
|
||||
return ntlVector4Dim<Scalar> ( ( Scalar ) 0 );
|
||||
}
|
||||
|
||||
//! Compute the norm of the vector and normalize it.
|
||||
/*! \return The value of the norm */
|
||||
template<class Scalar>
|
||||
inline Scalar normalize ( vector4Dim<Scalar> &v ) {
|
||||
inline Scalar normalize ( ntlVector4Dim<Scalar> &v ) {
|
||||
Scalar norm;
|
||||
Scalar l = v.x*v.x + v.y*v.y + v.z*v.z + v.t*v.t;
|
||||
if ( fabs ( l-1. ) < VECTOR_EPSILON*VECTOR_EPSILON ) {
|
||||
@@ -391,14 +391,14 @@ inline Scalar normalize ( vector4Dim<Scalar> &v ) {
|
||||
norm = sqrt ( l );
|
||||
v *= 1./norm;
|
||||
} else {
|
||||
v = vector4Dim<Scalar>::ZERO;
|
||||
v = ntlVector4Dim<Scalar>::ZERO;
|
||||
norm = 0.;
|
||||
}
|
||||
return ( Scalar ) norm;
|
||||
}
|
||||
|
||||
template<class Scalar>
|
||||
inline bool equal(const vector4Dim<Scalar> &v, const vector4Dim<Scalar> &c)
|
||||
inline bool equal(const ntlVector4Dim<Scalar> &v, const ntlVector4Dim<Scalar> &c)
|
||||
{
|
||||
return (ABS(v[0]-c[0]) +
|
||||
ABS(v[1]-c[1]) +
|
||||
@@ -407,7 +407,7 @@ inline bool equal(const vector4Dim<Scalar> &v, const vector4Dim<Scalar> &c)
|
||||
}
|
||||
|
||||
//! Outputs the object in human readable form as string
|
||||
template<class Scalar> std::string vector4Dim<Scalar>::toString() const {
|
||||
template<class Scalar> std::string ntlVector4Dim<Scalar>::toString() const {
|
||||
char buf[256];
|
||||
snprintf ( buf,256,"<%f,%f,%f,%f>", ( double ) ( *this ) [0], ( double ) ( *this ) [1], ( double ) ( *this ) [2] , ( double ) ( *this ) [3] );
|
||||
return std::string ( buf );
|
||||
@@ -415,7 +415,7 @@ template<class Scalar> std::string vector4Dim<Scalar>::toString() const {
|
||||
|
||||
//! Outputs the object in human readable form to stream
|
||||
template<class Scalar>
|
||||
std::ostream& operator<< ( std::ostream& os, const vector4Dim<Scalar>& i ) {
|
||||
std::ostream& operator<< ( std::ostream& os, const ntlVector4Dim<Scalar>& i ) {
|
||||
char buf[256];
|
||||
snprintf ( buf,256,"[%d,%d,%d,%d]", (double) i[0], (double) i[1], (double) i[2] , (double) i[3] );
|
||||
os << std::string ( buf );
|
||||
@@ -424,7 +424,7 @@ std::ostream& operator<< ( std::ostream& os, const vector4Dim<Scalar>& i ) {
|
||||
|
||||
//! Reads the contents of the object from a stream
|
||||
template<class Scalar>
|
||||
std::istream& operator>> ( std::istream& is, vector4Dim<Scalar>& i ) {
|
||||
std::istream& operator>> ( std::istream& is, ntlVector4Dim<Scalar>& i ) {
|
||||
char c;
|
||||
char dummy[4];
|
||||
is >> c >> i[0] >> dummy >> i[1] >> dummy >> i[2] >> dummy >> i[3] >> c;
|
||||
@@ -436,16 +436,16 @@ std::istream& operator>> ( std::istream& is, vector4Dim<Scalar>& i ) {
|
||||
/**************************************************************************/
|
||||
|
||||
//! 3D vector class of type Real (typically float)
|
||||
typedef vector4Dim<Real> Vec4;
|
||||
typedef ntlVector4Dim<Real> Vec4;
|
||||
|
||||
// a 3D vector with double precision
|
||||
typedef vector4Dim<double> nVec4d;
|
||||
typedef ntlVector4Dim<double> nVec4d;
|
||||
|
||||
// a 3D vector with single precision
|
||||
typedef vector4Dim<float> nVec4f;
|
||||
typedef ntlVector4Dim<float> nVec4f;
|
||||
|
||||
//! 3D vector class of type int
|
||||
typedef vector4Dim<int> nVec4i;
|
||||
typedef ntlVector4Dim<int> nVec4i;
|
||||
|
||||
/* convert int,float and double vectors */
|
||||
template<class T> inline nVec4i vec42I(T v) { return nVec4i((int)v[0],(int)v[1],(int)v[2],(int)v[3]); }
|
||||
|
||||
@@ -1,611 +0,0 @@
|
||||
/******************************************************************************
|
||||
*
|
||||
* MantaFlow fluid solver framework
|
||||
* Copyright 2011-2016 Tobias Pfaff, Nils Thuerey
|
||||
*
|
||||
* This program is free software, distributed under the terms of the
|
||||
* GNU General Public License (GPL)
|
||||
* http://www.gnu.org/licenses
|
||||
*
|
||||
* Basic vector class
|
||||
*
|
||||
******************************************************************************/
|
||||
|
||||
#ifndef _VECTORBASE_H
|
||||
#define _VECTORBASE_H
|
||||
|
||||
// get rid of windos min/max defines
|
||||
#if defined(WIN32) || defined(_WIN32)
|
||||
# define NOMINMAX
|
||||
#endif
|
||||
|
||||
#include <stdio.h>
|
||||
#include <string>
|
||||
#include <limits>
|
||||
#include <iostream>
|
||||
#include "general.h"
|
||||
|
||||
// if min/max are still around...
|
||||
#if defined(WIN32) || defined(_WIN32)
|
||||
# undef min
|
||||
# undef max
|
||||
#endif
|
||||
|
||||
// redefine usage of some windows functions
|
||||
#if defined(WIN32) || defined(_WIN32)
|
||||
# ifndef snprintf
|
||||
# define snprintf _snprintf
|
||||
# endif
|
||||
#endif
|
||||
|
||||
// use which fp-precision? 1=float, 2=double
|
||||
#ifndef FLOATINGPOINT_PRECISION
|
||||
# define FLOATINGPOINT_PRECISION 1
|
||||
#endif
|
||||
|
||||
// VECTOR_EPSILON is the minimal vector length
|
||||
// In order to be able to discriminate floating point values near zero, and
|
||||
// to be sure not to fail a comparison because of roundoff errors, use this
|
||||
// value as a threshold.
|
||||
#if FLOATINGPOINT_PRECISION==1
|
||||
typedef float Real;
|
||||
# define VECTOR_EPSILON (1e-6f)
|
||||
#else
|
||||
typedef double Real;
|
||||
# define VECTOR_EPSILON (1e-10)
|
||||
#endif
|
||||
|
||||
#ifndef M_PI
|
||||
# define M_PI 3.1415926536
|
||||
#endif
|
||||
#ifndef M_E
|
||||
# define M_E 2.7182818284
|
||||
#endif
|
||||
|
||||
namespace Manta
|
||||
{
|
||||
|
||||
//! Basic inlined vector class
|
||||
template<class S>
|
||||
class Vector3D
|
||||
{
|
||||
public:
|
||||
//! Constructor
|
||||
inline Vector3D() : x(0),y(0),z(0) {}
|
||||
|
||||
//! Copy-Constructor
|
||||
inline Vector3D ( const Vector3D<S> &v ) : x(v.x), y(v.y), z(v.z) {}
|
||||
|
||||
//! Copy-Constructor
|
||||
inline Vector3D ( const float * v) : x((S)v[0]), y((S)v[1]), z((S)v[2]) {}
|
||||
|
||||
//! Copy-Constructor
|
||||
inline Vector3D ( const double * v) : x((S)v[0]), y((S)v[1]), z((S)v[2]) {}
|
||||
|
||||
//! Construct a vector from one S
|
||||
inline Vector3D ( S v) : x(v), y(v), z(v) {}
|
||||
|
||||
//! Construct a vector from three Ss
|
||||
inline Vector3D ( S vx, S vy, S vz) : x(vx), y(vy), z(vz) {}
|
||||
|
||||
// Operators
|
||||
|
||||
//! Assignment operator
|
||||
inline const Vector3D<S>& operator= ( const Vector3D<S>& v ) {
|
||||
x = v.x;
|
||||
y = v.y;
|
||||
z = v.z;
|
||||
return *this;
|
||||
}
|
||||
//! Assignment operator
|
||||
inline const Vector3D<S>& operator= ( S s ) {
|
||||
x = y = z = s;
|
||||
return *this;
|
||||
}
|
||||
//! Assign and add operator
|
||||
inline const Vector3D<S>& operator+= ( const Vector3D<S>& v ) {
|
||||
x += v.x;
|
||||
y += v.y;
|
||||
z += v.z;
|
||||
return *this;
|
||||
}
|
||||
//! Assign and add operator
|
||||
inline const Vector3D<S>& operator+= ( S s ) {
|
||||
x += s;
|
||||
y += s;
|
||||
z += s;
|
||||
return *this;
|
||||
}
|
||||
//! Assign and sub operator
|
||||
inline const Vector3D<S>& operator-= ( const Vector3D<S>& v ) {
|
||||
x -= v.x;
|
||||
y -= v.y;
|
||||
z -= v.z;
|
||||
return *this;
|
||||
}
|
||||
//! Assign and sub operator
|
||||
inline const Vector3D<S>& operator-= ( S s ) {
|
||||
x -= s;
|
||||
y -= s;
|
||||
z -= s;
|
||||
return *this;
|
||||
}
|
||||
//! Assign and mult operator
|
||||
inline const Vector3D<S>& operator*= ( const Vector3D<S>& v ) {
|
||||
x *= v.x;
|
||||
y *= v.y;
|
||||
z *= v.z;
|
||||
return *this;
|
||||
}
|
||||
//! Assign and mult operator
|
||||
inline const Vector3D<S>& operator*= ( S s ) {
|
||||
x *= s;
|
||||
y *= s;
|
||||
z *= s;
|
||||
return *this;
|
||||
}
|
||||
//! Assign and div operator
|
||||
inline const Vector3D<S>& operator/= ( const Vector3D<S>& v ) {
|
||||
x /= v.x;
|
||||
y /= v.y;
|
||||
z /= v.z;
|
||||
return *this;
|
||||
}
|
||||
//! Assign and div operator
|
||||
inline const Vector3D<S>& operator/= ( S s ) {
|
||||
x /= s;
|
||||
y /= s;
|
||||
z /= s;
|
||||
return *this;
|
||||
}
|
||||
//! Negation operator
|
||||
inline Vector3D<S> operator- () const {
|
||||
return Vector3D<S> (-x, -y, -z);
|
||||
}
|
||||
|
||||
//! Get smallest component
|
||||
inline S min() const {
|
||||
return ( x<y ) ? ( ( x<z ) ? x:z ) : ( ( y<z ) ? y:z );
|
||||
}
|
||||
//! Get biggest component
|
||||
inline S max() const {
|
||||
return ( x>y ) ? ( ( x>z ) ? x:z ) : ( ( y>z ) ? y:z );
|
||||
}
|
||||
|
||||
//! Test if all components are zero
|
||||
inline bool empty() {
|
||||
return x==0 && y==0 && z==0;
|
||||
}
|
||||
|
||||
//! access operator
|
||||
inline S& operator[] ( unsigned int i ) {
|
||||
return value[i];
|
||||
}
|
||||
//! constant access operator
|
||||
inline const S& operator[] ( unsigned int i ) const {
|
||||
return value[i];
|
||||
}
|
||||
|
||||
//! debug output vector to a string
|
||||
std::string toString() const;
|
||||
|
||||
//! test if nans are present
|
||||
bool isValid() const;
|
||||
|
||||
//! actual values
|
||||
union {
|
||||
S value[3];
|
||||
struct {
|
||||
S x;
|
||||
S y;
|
||||
S z;
|
||||
};
|
||||
struct {
|
||||
S X;
|
||||
S Y;
|
||||
S Z;
|
||||
};
|
||||
};
|
||||
|
||||
//! zero element
|
||||
static const Vector3D<S> Zero, Invalid;
|
||||
|
||||
//! For compatibility with 4d vectors (discards 4th comp)
|
||||
inline Vector3D ( S vx, S vy, S vz, S vDummy) : x(vx), y(vy), z(vz) {}
|
||||
|
||||
protected:
|
||||
|
||||
};
|
||||
|
||||
//************************************************************************
|
||||
// Additional operators
|
||||
//************************************************************************
|
||||
|
||||
//! Addition operator
|
||||
template<class S>
|
||||
inline Vector3D<S> operator+ ( const Vector3D<S> &v1, const Vector3D<S> &v2 ) {
|
||||
return Vector3D<S> ( v1.x+v2.x, v1.y+v2.y, v1.z+v2.z );
|
||||
}
|
||||
//! Addition operator
|
||||
template<class S, class S2>
|
||||
inline Vector3D<S> operator+ ( const Vector3D<S>& v, S2 s ) {
|
||||
return Vector3D<S> ( v.x+s, v.y+s, v.z+s );
|
||||
}
|
||||
//! Addition operator
|
||||
template<class S, class S2>
|
||||
inline Vector3D<S> operator+ ( S2 s, const Vector3D<S>& v ) {
|
||||
return Vector3D<S> ( v.x+s, v.y+s, v.z+s );
|
||||
}
|
||||
|
||||
//! Subtraction operator
|
||||
template<class S>
|
||||
inline Vector3D<S> operator- ( const Vector3D<S> &v1, const Vector3D<S> &v2 ) {
|
||||
return Vector3D<S> ( v1.x-v2.x, v1.y-v2.y, v1.z-v2.z );
|
||||
}
|
||||
//! Subtraction operator
|
||||
template<class S, class S2>
|
||||
inline Vector3D<S> operator- ( const Vector3D<S>& v, S2 s ) {
|
||||
return Vector3D<S> ( v.x-s, v.y-s, v.z-s );
|
||||
}
|
||||
//! Subtraction operator
|
||||
template<class S, class S2>
|
||||
inline Vector3D<S> operator- ( S2 s, const Vector3D<S>& v ) {
|
||||
return Vector3D<S> ( s-v.x, s-v.y, s-v.z );
|
||||
}
|
||||
|
||||
//! Multiplication operator
|
||||
template<class S>
|
||||
inline Vector3D<S> operator* ( const Vector3D<S> &v1, const Vector3D<S> &v2 ) {
|
||||
return Vector3D<S> ( v1.x*v2.x, v1.y*v2.y, v1.z*v2.z );
|
||||
}
|
||||
//! Multiplication operator
|
||||
template<class S, class S2>
|
||||
inline Vector3D<S> operator* ( const Vector3D<S>& v, S2 s ) {
|
||||
return Vector3D<S> ( v.x*s, v.y*s, v.z*s );
|
||||
}
|
||||
//! Multiplication operator
|
||||
template<class S, class S2>
|
||||
inline Vector3D<S> operator* ( S2 s, const Vector3D<S>& v ) {
|
||||
return Vector3D<S> ( s*v.x, s*v.y, s*v.z );
|
||||
}
|
||||
|
||||
//! Division operator
|
||||
template<class S>
|
||||
inline Vector3D<S> operator/ ( const Vector3D<S> &v1, const Vector3D<S> &v2 ) {
|
||||
return Vector3D<S> ( v1.x/v2.x, v1.y/v2.y, v1.z/v2.z );
|
||||
}
|
||||
//! Division operator
|
||||
template<class S, class S2>
|
||||
inline Vector3D<S> operator/ ( const Vector3D<S>& v, S2 s ) {
|
||||
return Vector3D<S> ( v.x/s, v.y/s, v.z/s );
|
||||
}
|
||||
//! Division operator
|
||||
template<class S, class S2>
|
||||
inline Vector3D<S> operator/ ( S2 s, const Vector3D<S>& v ) {
|
||||
return Vector3D<S> ( s/v.x, s/v.y, s/v.z );
|
||||
}
|
||||
|
||||
//! Comparison operator
|
||||
template<class S>
|
||||
inline bool operator== (const Vector3D<S>& s1, const Vector3D<S>& s2) {
|
||||
return s1.x == s2.x && s1.y == s2.y && s1.z == s2.z;
|
||||
}
|
||||
|
||||
//! Comparison operator
|
||||
template<class S>
|
||||
inline bool operator!= (const Vector3D<S>& s1, const Vector3D<S>& s2) {
|
||||
return s1.x != s2.x || s1.y != s2.y || s1.z != s2.z;
|
||||
}
|
||||
|
||||
//************************************************************************
|
||||
// External functions
|
||||
//************************************************************************
|
||||
|
||||
//! Min operator
|
||||
template<class S>
|
||||
inline Vector3D<S> vmin (const Vector3D<S>& s1, const Vector3D<S>& s2) {
|
||||
return Vector3D<S>(std::min(s1.x,s2.x), std::min(s1.y,s2.y), std::min(s1.z,s2.z));
|
||||
}
|
||||
|
||||
//! Min operator
|
||||
template<class S, class S2>
|
||||
inline Vector3D<S> vmin (const Vector3D<S>& s1, S2 s2) {
|
||||
return Vector3D<S>(std::min(s1.x,s2), std::min(s1.y,s2), std::min(s1.z,s2));
|
||||
}
|
||||
|
||||
//! Min operator
|
||||
template<class S1, class S>
|
||||
inline Vector3D<S> vmin (S1 s1, const Vector3D<S>& s2) {
|
||||
return Vector3D<S>(std::min(s1,s2.x), std::min(s1,s2.y), std::min(s1,s2.z));
|
||||
}
|
||||
|
||||
//! Max operator
|
||||
template<class S>
|
||||
inline Vector3D<S> vmax (const Vector3D<S>& s1, const Vector3D<S>& s2) {
|
||||
return Vector3D<S>(std::max(s1.x,s2.x), std::max(s1.y,s2.y), std::max(s1.z,s2.z));
|
||||
}
|
||||
|
||||
//! Max operator
|
||||
template<class S, class S2>
|
||||
inline Vector3D<S> vmax (const Vector3D<S>& s1, S2 s2) {
|
||||
return Vector3D<S>(std::max(s1.x,s2), std::max(s1.y,s2), std::max(s1.z,s2));
|
||||
}
|
||||
|
||||
//! Max operator
|
||||
template<class S1, class S>
|
||||
inline Vector3D<S> vmax (S1 s1, const Vector3D<S>& s2) {
|
||||
return Vector3D<S>(std::max(s1,s2.x), std::max(s1,s2.y), std::max(s1,s2.z));
|
||||
}
|
||||
|
||||
//! Dot product
|
||||
template<class S>
|
||||
inline S dot ( const Vector3D<S> &t, const Vector3D<S> &v ) {
|
||||
return t.x*v.x + t.y*v.y + t.z*v.z;
|
||||
}
|
||||
|
||||
//! Cross product
|
||||
template<class S>
|
||||
inline Vector3D<S> cross ( const Vector3D<S> &t, const Vector3D<S> &v ) {
|
||||
Vector3D<S> cp (
|
||||
( ( t.y*v.z ) - ( t.z*v.y ) ),
|
||||
( ( t.z*v.x ) - ( t.x*v.z ) ),
|
||||
( ( t.x*v.y ) - ( t.y*v.x ) ) );
|
||||
return cp;
|
||||
}
|
||||
|
||||
//! Project a vector into a plane, defined by its normal
|
||||
/*! Projects a vector into a plane normal to the given vector, which must
|
||||
have unit length. Self is modified.
|
||||
\param v The vector to project
|
||||
\param n The plane normal
|
||||
\return The projected vector */
|
||||
template<class S>
|
||||
inline const Vector3D<S>& projectNormalTo ( const Vector3D<S>& v, const Vector3D<S> &n) {
|
||||
S sprod = dot (v, n);
|
||||
return v - n * dot(v, n);
|
||||
}
|
||||
|
||||
//! Compute the magnitude (length) of the vector
|
||||
//! (clamps to 0 and 1 with VECTOR_EPSILON)
|
||||
template<class S>
|
||||
inline S norm ( const Vector3D<S>& v ) {
|
||||
S l = v.x*v.x + v.y*v.y + v.z*v.z;
|
||||
if ( l <= VECTOR_EPSILON*VECTOR_EPSILON ) return(0.);
|
||||
return ( fabs ( l-1. ) < VECTOR_EPSILON*VECTOR_EPSILON ) ? 1. : sqrt ( l );
|
||||
}
|
||||
|
||||
//! Compute squared magnitude
|
||||
template<class S>
|
||||
inline S normSquare ( const Vector3D<S>& v ) {
|
||||
return v.x*v.x + v.y*v.y + v.z*v.z;
|
||||
}
|
||||
|
||||
//! compatibility, allow use of int, Real and Vec inputs with norm/normSquare
|
||||
inline Real norm(const Real v) { return fabs(v); }
|
||||
inline Real normSquare(const Real v) { return square(v); }
|
||||
inline Real norm(const int v) { return abs(v); }
|
||||
inline Real normSquare(const int v) { return square(v); }
|
||||
|
||||
//! Returns a normalized vector
|
||||
template<class S>
|
||||
inline Vector3D<S> getNormalized ( const Vector3D<S>& v ) {
|
||||
S l = v.x*v.x + v.y*v.y + v.z*v.z;
|
||||
if ( fabs ( l-1. ) < VECTOR_EPSILON*VECTOR_EPSILON )
|
||||
return v; /* normalized "enough"... */
|
||||
else if ( l > VECTOR_EPSILON*VECTOR_EPSILON )
|
||||
{
|
||||
S fac = 1./sqrt ( l );
|
||||
return Vector3D<S> ( v.x*fac, v.y*fac, v.z*fac );
|
||||
}
|
||||
else
|
||||
return Vector3D<S> ( ( S ) 0 );
|
||||
}
|
||||
|
||||
//! Compute the norm of the vector and normalize it.
|
||||
/*! \return The value of the norm */
|
||||
template<class S>
|
||||
inline S normalize ( Vector3D<S> &v ) {
|
||||
S norm;
|
||||
S l = v.x*v.x + v.y*v.y + v.z*v.z;
|
||||
if ( fabs ( l-1. ) < VECTOR_EPSILON*VECTOR_EPSILON ) {
|
||||
norm = 1.;
|
||||
} else if ( l > VECTOR_EPSILON*VECTOR_EPSILON ) {
|
||||
norm = sqrt ( l );
|
||||
v *= 1./norm;
|
||||
} else {
|
||||
v = Vector3D<S>::Zero;
|
||||
norm = 0.;
|
||||
}
|
||||
return ( S ) norm;
|
||||
}
|
||||
|
||||
//! Obtain an orthogonal vector
|
||||
/*! Compute a vector that is orthonormal to the given vector.
|
||||
* Nothing else can be assumed for the direction of the new vector.
|
||||
* \return The orthonormal vector */
|
||||
template<class S>
|
||||
Vector3D<S> getOrthogonalVector(const Vector3D<S>& v) {
|
||||
// Determine the component with max. absolute value
|
||||
int maxIndex= ( fabs ( v.x ) > fabs ( v.y ) ) ? 0 : 1;
|
||||
maxIndex= ( fabs ( v[maxIndex] ) > fabs ( v.z ) ) ? maxIndex : 2;
|
||||
|
||||
// Choose another axis than the one with max. component and project
|
||||
// orthogonal to self
|
||||
Vector3D<S> o ( 0.0 );
|
||||
o[ ( maxIndex+1 ) %3]= 1;
|
||||
|
||||
Vector3D<S> c = cross(v, o);
|
||||
normalize(c);
|
||||
return c;
|
||||
}
|
||||
|
||||
//! Convert vector to polar coordinates
|
||||
/*! Stable vector to angle conversion
|
||||
*\param v vector to convert
|
||||
\param phi unique angle [0,2PI]
|
||||
\param theta unique angle [0,PI]
|
||||
*/
|
||||
template<class S>
|
||||
inline void vecToAngle ( const Vector3D<S>& v, S& phi, S& theta )
|
||||
{
|
||||
if ( fabs ( v.y ) < VECTOR_EPSILON )
|
||||
theta = M_PI/2;
|
||||
else if ( fabs ( v.x ) < VECTOR_EPSILON && fabs ( v.z ) < VECTOR_EPSILON )
|
||||
theta = ( v.y>=0 ) ? 0:M_PI;
|
||||
else
|
||||
theta = atan ( sqrt ( v.x*v.x+v.z*v.z ) /v.y );
|
||||
if ( theta<0 ) theta+=M_PI;
|
||||
|
||||
if ( fabs ( v.x ) < VECTOR_EPSILON )
|
||||
phi = M_PI/2;
|
||||
else
|
||||
phi = atan ( v.z/v.x );
|
||||
if ( phi<0 ) phi+=M_PI;
|
||||
if ( fabs ( v.z ) < VECTOR_EPSILON )
|
||||
phi = ( v.x>=0 ) ? 0 : M_PI;
|
||||
else if ( v.z < 0 )
|
||||
phi += M_PI;
|
||||
}
|
||||
|
||||
//! Compute vector reflected at a surface
|
||||
/*! Compute a vector, that is self (as an incoming vector)
|
||||
* reflected at a surface with a distinct normal vector.
|
||||
* Note that the normal is reversed, if the scalar product with it is positive.
|
||||
\param t The incoming vector
|
||||
\param n The surface normal
|
||||
\return The new reflected vector
|
||||
*/
|
||||
template<class S>
|
||||
inline Vector3D<S> reflectVector ( const Vector3D<S>& t, const Vector3D<S>& n ) {
|
||||
Vector3D<S> nn= ( dot ( t, n ) > 0.0 ) ? ( n*-1.0 ) : n;
|
||||
return ( t - nn * ( 2.0 * dot ( nn, t ) ) );
|
||||
}
|
||||
|
||||
//! Compute vector refracted at a surface
|
||||
/*! \param t The incoming vector
|
||||
* \param n The surface normal
|
||||
* \param nt The "inside" refraction index
|
||||
* \param nair The "outside" refraction index
|
||||
* \param refRefl Set to 1 on total reflection
|
||||
* \return The refracted vector
|
||||
*/
|
||||
template<class S>
|
||||
inline Vector3D<S> refractVector ( const Vector3D<S> &t, const Vector3D<S> &normal, S nt, S nair, int &refRefl ) {
|
||||
// from Glassner's book, section 5.2 (Heckberts method)
|
||||
S eta = nair / nt;
|
||||
S n = -dot ( t, normal );
|
||||
S tt = 1.0 + eta*eta* ( n*n-1.0 );
|
||||
if ( tt<0.0 ) {
|
||||
// we have total reflection!
|
||||
refRefl = 1;
|
||||
} else {
|
||||
// normal reflection
|
||||
tt = eta*n - sqrt ( tt );
|
||||
return ( t*eta + normal*tt );
|
||||
}
|
||||
return t;
|
||||
}
|
||||
|
||||
//! Outputs the object in human readable form as string
|
||||
template<class S> std::string Vector3D<S>::toString() const {
|
||||
char buf[256];
|
||||
snprintf ( buf,256,"[%+4.6f,%+4.6f,%+4.6f]", ( double ) ( *this ) [0], ( double ) ( *this ) [1], ( double ) ( *this ) [2] );
|
||||
// for debugging, optionally increase precision:
|
||||
//snprintf ( buf,256,"[%+4.16f,%+4.16f,%+4.16f]", ( double ) ( *this ) [0], ( double ) ( *this ) [1], ( double ) ( *this ) [2] );
|
||||
return std::string ( buf );
|
||||
}
|
||||
|
||||
template<> std::string Vector3D<int>::toString() const;
|
||||
|
||||
|
||||
//! Outputs the object in human readable form to stream
|
||||
/*! Output format [x,y,z] */
|
||||
template<class S>
|
||||
std::ostream& operator<< ( std::ostream& os, const Vector3D<S>& i ) {
|
||||
os << i.toString();
|
||||
return os;
|
||||
}
|
||||
|
||||
//! Reads the contents of the object from a stream
|
||||
/*! Input format [x,y,z] */
|
||||
template<class S>
|
||||
std::istream& operator>> ( std::istream& is, Vector3D<S>& i ) {
|
||||
char c;
|
||||
char dummy[3];
|
||||
is >> c >> i[0] >> dummy >> i[1] >> dummy >> i[2] >> c;
|
||||
return is;
|
||||
}
|
||||
|
||||
/**************************************************************************/
|
||||
// Define default vector alias
|
||||
/**************************************************************************/
|
||||
|
||||
//! 3D vector class of type Real (typically float)
|
||||
typedef Vector3D<Real> Vec3;
|
||||
|
||||
//! 3D vector class of type int
|
||||
typedef Vector3D<int> Vec3i;
|
||||
|
||||
//! convert to Real Vector
|
||||
template<class T> inline Vec3 toVec3 ( T v ) {
|
||||
return Vec3 ( v[0],v[1],v[2] );
|
||||
}
|
||||
|
||||
//! convert to int Vector
|
||||
template<class T> inline Vec3i toVec3i ( T v ) {
|
||||
return Vec3i ( ( int ) v[0], ( int ) v[1], ( int ) v[2] );
|
||||
}
|
||||
|
||||
//! convert to int Vector
|
||||
template<class T> inline Vec3i toVec3i ( T v0, T v1, T v2 ) {
|
||||
return Vec3i ( ( int ) v0, ( int ) v1, ( int ) v2 );
|
||||
}
|
||||
|
||||
//! round, and convert to int Vector
|
||||
template<class T> inline Vec3i toVec3iRound ( T v ) {
|
||||
return Vec3i ( ( int ) round ( v[0] ), ( int ) round ( v[1] ), ( int ) round ( v[2] ) );
|
||||
}
|
||||
|
||||
//! convert to int Vector if values are close enough to an int
|
||||
template<class T> inline Vec3i toVec3iChecked ( T v ) {
|
||||
Vec3i ret;
|
||||
for (size_t i=0; i<3; i++) {
|
||||
Real a = v[i];
|
||||
if (fabs(a-floor(a+0.5)) > 1e-5)
|
||||
errMsg("argument is not an int, cannot convert");
|
||||
ret[i] = (int) (a+0.5);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
//! convert to double Vector
|
||||
template<class T> inline Vector3D<double> toVec3d ( T v ) {
|
||||
return Vector3D<double> ( v[0], v[1], v[2] );
|
||||
}
|
||||
|
||||
//! convert to float Vector
|
||||
template<class T> inline Vector3D<float> toVec3f ( T v ) {
|
||||
return Vector3D<float> ( v[0], v[1], v[2] );
|
||||
}
|
||||
|
||||
|
||||
/**************************************************************************/
|
||||
// Specializations for common math functions
|
||||
/**************************************************************************/
|
||||
|
||||
template<> inline Vec3 clamp<Vec3>(const Vec3& a, const Vec3& b, const Vec3& c) {
|
||||
return Vec3 ( clamp(a.x, b.x, c.x),
|
||||
clamp(a.y, b.y, c.y),
|
||||
clamp(a.z, b.z, c.z) );
|
||||
}
|
||||
template<> inline Vec3 safeDivide<Vec3>(const Vec3 &a, const Vec3& b) {
|
||||
return Vec3(safeDivide(a.x,b.x), safeDivide(a.y,b.y), safeDivide(a.z,b.z));
|
||||
}
|
||||
template<> inline Vec3 nmod<Vec3>(const Vec3& a, const Vec3& b) {
|
||||
return Vec3(nmod(a.x,b.x),nmod(a.y,b.y),nmod(a.z,b.z));
|
||||
}
|
||||
|
||||
}; // namespace
|
||||
|
||||
|
||||
#endif
|
||||
105
SimulatorTester/PublicRigidBodiesTests.cpp
Normal file
105
SimulatorTester/PublicRigidBodiesTests.cpp
Normal file
@@ -0,0 +1,105 @@
|
||||
#include "CppUnitTest.h"
|
||||
#include "RigidBodySystemSimulator.h"
|
||||
|
||||
using namespace Microsoft::VisualStudio::CppUnitTestFramework;
|
||||
|
||||
namespace SimulatorTester
|
||||
{
|
||||
TEST_CLASS(PublicRigidBodiesTests)
|
||||
{
|
||||
public:
|
||||
void setupBaseTest(RigidBodySystemSimulator * rbss) {
|
||||
rbss->m_iTestCase = TESTCASEUSEDTORUNTEST;
|
||||
rbss->addRigidBody(Vec3(-0.1f, -0.2f, 0.1f), Vec3(0.4f, 0.2f, 0.2f), 100.0f);
|
||||
|
||||
rbss->addRigidBody(Vec3(0.0f, 0.2f, 0.0f), Vec3(0.4f, 0.2f, 0.2f), 100.0);
|
||||
rbss->setOrientationOf(1, Quat(Vec3(0.0f, 0.0f, 1.0f), (float)(M_PI)*0.25f));
|
||||
rbss->setVelocityOf(1,Vec3(0.0f, -0.1f, 0.05f));
|
||||
}
|
||||
|
||||
TEST_METHOD(TestRigidBodiesInitialization)
|
||||
{
|
||||
RigidBodySystemSimulator * rbss = new RigidBodySystemSimulator();
|
||||
setupBaseTest(rbss);
|
||||
Assert::AreEqual(2,(int)rbss->getNumberOfRigidBodies(),0.0001f,L"Number of Rigid bodies is not right",LINE_INFO());
|
||||
Vec3 pos = rbss->getPositionOfRigidBody(0);
|
||||
Assert::AreEqual(-0.1f,(float)pos.x,0.0001f,L"X coordinate of body 0 is not right",LINE_INFO());
|
||||
Assert::AreEqual(-0.2f,(float)pos.y,0.0001f,L"Y coordinate of body 0 is not right",LINE_INFO());
|
||||
Assert::AreEqual(0.1f,(float)pos.z,0.0001f,L"Z coordinate of body 0 is not right",LINE_INFO());
|
||||
Vec3 vel = rbss->getLinearVelocityOfRigidBody(0);
|
||||
Assert::AreEqual(0.0f,(float)vel.x,0.0001f,L"X componnent of body 0 is not right",LINE_INFO());
|
||||
Assert::AreEqual(0.0f,(float)vel.y,0.0001f,L"Y componnent of body 0 is not right",LINE_INFO());
|
||||
Assert::AreEqual(0.0f,(float)vel.z,0.0001f,L"Z componnent of body 0 is not right",LINE_INFO());
|
||||
Vec3 angvel = rbss->getAngularVelocityOfRigidBody(0);
|
||||
Assert::AreEqual(0.0f,(float)angvel.x,0.0001f,L"X componnent of body 0 is not right",LINE_INFO());
|
||||
Assert::AreEqual(0.0f,(float)angvel.y,0.0001f,L"Y componnent of body 0 is not right",LINE_INFO());
|
||||
Assert::AreEqual(0.0f,(float)angvel.z,0.0001f,L"Z componnent of body 0 is not right",LINE_INFO());
|
||||
pos = rbss->getPositionOfRigidBody(1);
|
||||
Assert::AreEqual(0.0f,(float)pos.x,0.0001f,L"X coordinate of body 1 is not right",LINE_INFO());
|
||||
Assert::AreEqual(0.2f,(float)pos.y,0.0001f,L"Y coordinate of body 1 is not right",LINE_INFO());
|
||||
Assert::AreEqual(0.0f,(float)pos.z,0.0001f,L"Z coordinate of body 1 is not right",LINE_INFO());
|
||||
vel = rbss->getLinearVelocityOfRigidBody(1);
|
||||
Assert::AreEqual(0.0f,(float)vel.x,0.0001f,L"X componnent of body 1 is not right",LINE_INFO());
|
||||
Assert::AreEqual(-0.1f,(float)vel.y,0.0001f,L"Y componnent of body 1 is not right",LINE_INFO());
|
||||
Assert::AreEqual(0.05f,(float)vel.z,0.0001f,L"Z componnent of body 1 is not right",LINE_INFO());
|
||||
angvel = rbss->getAngularVelocityOfRigidBody(1);
|
||||
Assert::AreEqual(0.0f,(float)angvel.x,0.0001f,L"X componnent of body 0 is not right",LINE_INFO());
|
||||
Assert::AreEqual(0.0f,(float)angvel.y,0.0001f,L"Y componnent of body 0 is not right",LINE_INFO());
|
||||
Assert::AreEqual(0.0f,(float)angvel.z,0.0001f,L"Z componnent of body 0 is not right",LINE_INFO());
|
||||
delete rbss;
|
||||
}
|
||||
|
||||
TEST_METHOD(TestRigidBodiesAfterForceApplication)
|
||||
{
|
||||
RigidBodySystemSimulator * rbss = new RigidBodySystemSimulator();
|
||||
setupBaseTest(rbss);
|
||||
rbss->applyForceOnBody(0,Vec3(0.0,0.0f,0.0),Vec3(0,0,200));
|
||||
for(int i =0; i < 4;i++)
|
||||
rbss->simulateTimestep(0.1);
|
||||
Vec3 pos = rbss->getPositionOfRigidBody(0);
|
||||
Assert::AreEqual(-0.1f,(float)pos.x,0.0001f,L"X coordinate of body 0 is not right",LINE_INFO());
|
||||
Assert::AreEqual(-0.2f,(float)pos.y,0.0001f,L"Y coordinate of body 0 is not right",LINE_INFO());
|
||||
Assert::AreEqual(0.16f,(float)pos.z,0.0001f,L"Z coordinate of body 0 is not right",LINE_INFO());
|
||||
Vec3 vel = rbss->getLinearVelocityOfRigidBody(0);
|
||||
Assert::AreEqual(0.0f,(float)vel.x,0.0001f,L"X componnent of body 0 is not right",LINE_INFO());
|
||||
Assert::AreEqual(0.0f,(float)vel.y,0.0001f,L"Y componnent of body 0 is not right",LINE_INFO());
|
||||
Assert::AreEqual(0.2f,(float)vel.z,0.0001f,L"Z componnent of body 0 is not right",LINE_INFO());
|
||||
Vec3 angvel = rbss->getAngularVelocityOfRigidBody(0);
|
||||
Assert::AreEqual(5.8590f,(float)angvel.x,0.0001f,L"X componnent of body 0 is not right",LINE_INFO());
|
||||
Assert::AreEqual(-2.1106f,(float)angvel.y,0.0001f,L"Y componnent of body 0 is not right",LINE_INFO());
|
||||
Assert::AreEqual(-1.1391f,(float)angvel.z,0.0001f,L"Z componnent of body 0 is not right",LINE_INFO());
|
||||
delete rbss;
|
||||
}
|
||||
|
||||
TEST_METHOD(TestRigidBodiesOneStepGivenTableTest)
|
||||
{
|
||||
RigidBodySystemSimulator * rbss = new RigidBodySystemSimulator();
|
||||
|
||||
rbss->m_iTestCase = TESTCASEUSEDTORUNTEST;
|
||||
rbss->addRigidBody(Vec3(0.0f, 0.0f, 0.0f), Vec3(1.0f, 0.6f, 0.5f), 2.0f);
|
||||
rbss->setOrientationOf(0, Quat(Vec3(0.0f, 0.0f, 1.0f), (float)(M_PI)* 0.5f));
|
||||
rbss->applyForceOnBody(0, Vec3(0.3f, 0.5f, 0.25f), Vec3(1.0f, 1.0f, 0.0f));
|
||||
rbss->simulateTimestep(2.0);
|
||||
|
||||
Vec3 pos = rbss->getPositionOfRigidBody(0);
|
||||
Assert::AreEqual(0.0000f, (float)pos.x, 0.0001f, L"X coordinate of position of body 0 is not right", LINE_INFO());
|
||||
Assert::AreEqual(0.0000f, (float)pos.y, 0.0001f, L"Y coordinate of position of body 0 is not right", LINE_INFO());
|
||||
Assert::AreEqual(0.0000f, (float)pos.z, 0.0001f, L"Z coordinate of position of body 0 is not right", LINE_INFO());
|
||||
Vec3 vel = rbss->getLinearVelocityOfRigidBody(0);
|
||||
Assert::AreEqual(1.0000f, (float)vel.x, 0.0001f, L"X componnent of velocity of body 0 is not right", LINE_INFO());
|
||||
Assert::AreEqual(1.0000f, (float)vel.y, 0.0001f, L"Y componnent of velocity of body 0 is not right", LINE_INFO());
|
||||
Assert::AreEqual(0.0000f, (float)vel.z, 0.0001f, L"Z componnent of velocity of body 0 is not right", LINE_INFO());
|
||||
Vec3 angvel = rbss->getAngularVelocityOfRigidBody(0);
|
||||
Assert::AreEqual(-2.4000f, (float)angvel.x, 0.0001f, L"X componnent of angular velocity of body 0 is not right", LINE_INFO());
|
||||
Assert::AreEqual(4.9180f, (float)angvel.y, 0.0001f, L"Y componnent of angular velocity of body 0 is not right", LINE_INFO());
|
||||
Assert::AreEqual(-1.7647f, (float)angvel.z, 0.0001f, L"Z componnent of angular velocity of body 0 is not right", LINE_INFO());
|
||||
|
||||
Vec3 xa_world = Vec3(-0.3f, -0.5f, -0.25f) - pos;
|
||||
Vec3 velocityA = vel + cross(angvel, xa_world);
|
||||
Assert::AreEqual(-1.11186f, (float)velocityA.x, 0.0001f, L"X componnent of the velocity at the given point is not right", LINE_INFO());
|
||||
Assert::AreEqual(0.929412f, (float)velocityA.y, 0.0001f, L"Y componnent of the velocity at the given point is not right", LINE_INFO());
|
||||
Assert::AreEqual(2.67541f, (float)velocityA.z, 0.0001f, L"Z componnent of the velocity at the given point is not right", LINE_INFO());
|
||||
delete rbss;
|
||||
}
|
||||
};
|
||||
}
|
||||
@@ -163,6 +163,7 @@
|
||||
</ItemDefinitionGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="PublicMassSpringSystemTests.cpp" />
|
||||
<ClCompile Include="PublicRigidBodiesTests.cpp" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\AntTweakBar\src\AntTweakBar_2013.vcxproj">
|
||||
|
||||
@@ -163,6 +163,7 @@
|
||||
</ItemDefinitionGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="PublicMassSpringSystemTests.cpp" />
|
||||
<ClCompile Include="PublicRigidBodiesTests.cpp" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\AntTweakBar\src\AntTweakBar_2015.vcxproj">
|
||||
|
||||
@@ -164,6 +164,7 @@
|
||||
</ItemDefinitionGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="PublicMassSpringSystemTests.cpp" />
|
||||
<ClCompile Include="PublicRigidBodiesTests.cpp" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\AntTweakBar\src\AntTweakBar_2017.vcxproj">
|
||||
|
||||
Reference in New Issue
Block a user