kookroach/game-physics
1
0
Fork 0
Code Issues 8 Pull Requests Packages Projects 1 Releases Wiki Activity

template project, first version

Browse Source
This commit is contained in:
rachelchu
2017-10-11 15:01:05 +02:00
commit 8e902224cf
794 changed files with 326190 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

220
Simulations/util/FFmpeg.cpp Normal file
View File

@@ -0,0 +1,220 @@
#include "FFmpeg.h"
#include <iostream>
#include <sstream>
#include <d3d11_1.h>
bool FFmpeg::ms_bFFmpegInstalled = false;
FFmpeg::FFmpeg(int fps, int crf, MODE mode)
: m_pFILE(nullptr),
m_pStaging(nullptr),
m_width(-1),
m_height(-1),
m_mode(mode),
m_fps(fps),
m_crf(crf),
m_frame(-1)
{
static bool s_checkPerformed = false;
m_timestamp[0].QuadPart = m_timestamp[1].QuadPart = 0;
m_frequency.QuadPart = 0;
m_startTime.QuadPart = 0;
// Check once if ffmpeg.exe is installed, i.e. if it's in system path
if (!s_checkPerformed)
{
FILE* file = _popen("ffmpeg -version", "rt");
if (_pclose(file) == 0)
{
ms_bFFmpegInstalled = true;
}
else
{
ms_bFFmpegInstalled = false;
std::cout << "Warning: ffmpeg.exe not found, video recording disabled..." << std::endl;
}
s_checkPerformed = true;
}
}
FFmpeg::~FFmpeg()
{
StopRecording();
}
HRESULT FFmpeg::StartRecording(ID3D11Device* pd3dDevice, ID3D11RenderTargetView* pRenderTargetView, char* filename)
{
if (!ms_bFFmpegInstalled) { return S_OK; }
HRESULT hr;
std::stringstream cmd;
// 1) Create a staging texture from the non-MSAA source
ID3D11Resource* pResource = nullptr;
pRenderTargetView->GetResource(&pResource);
ID3D11Texture2D* pTexture = nullptr;
hr = pResource->QueryInterface( __uuidof(ID3D11Texture2D), (void**) &pTexture );
if (hr != S_OK) { goto StartRecording_Return; }
D3D11_TEXTURE2D_DESC desc;
pTexture->GetDesc( &desc );
if (desc.Format != DXGI_FORMAT_R8G8B8A8_TYPELESS &&
desc.Format != DXGI_FORMAT_R8G8B8A8_UNORM &&
desc.Format != DXGI_FORMAT_R8G8B8A8_UNORM_SRGB &&
desc.Format != DXGI_FORMAT_R8G8B8A8_UINT &&
desc.Format != DXGI_FORMAT_R8G8B8A8_SNORM &&
desc.Format != DXGI_FORMAT_R8G8B8A8_SINT)
{
std::cout << "FFmpeg error: Currently only R8G8B8A8 render targets are supported." << std::endl;
hr = E_INVALIDARG;
goto StartRecording_Return;
}
desc.BindFlags = 0;
desc.CPUAccessFlags = D3D11_CPU_ACCESS_READ;
desc.Usage = D3D11_USAGE_STAGING;
hr = pd3dDevice->CreateTexture2D( &desc, 0, &m_pStaging );
if (hr != S_OK) { goto StartRecording_Return; }
// 2) Spawn ffmpeg process
// For h264 control options, see https://trac.ffmpeg.org/wiki/Encode/H.264
cmd << "ffmpeg "
"-f rawvideo -pix_fmt rgba " // raw rgba input video format
"-s " << desc.Width << "x" << desc.Height << " " // input resolution
"-r " << m_fps << " " // input framerate
" -i - " // read input from stdin
"-y " // auto-overwrite output file
"-codec:video libx264 " // use h264 codec
"-preset fast " // encoder preset (ultrafast,superfast, veryfast, faster, fast, medium, slow, slower, veryslow)
"-crf " << m_crf << " " // constant rate factor
<< filename;
// open pipe to ffmpeg's stdin in binary write mode
m_pFILE = _popen(cmd.str().c_str(), "wb");
// 3) Allocate buffers
m_width = desc.Width;
m_height = desc.Height;
m_buffer[0].resize(m_width * m_height * sizeof(uint32_t));
m_buffer[1].resize(m_width * m_height * sizeof(uint32_t));
m_buffer[2].resize(m_width * m_height * sizeof(uint32_t));
m_frame = -1;
StartRecording_Return:
if (pResource) {pResource->Release(); pResource = nullptr; }
if (pTexture ) {pTexture ->Release(); pTexture = nullptr; }
return hr;
}
void FFmpeg::StopRecording()
{
// Release everything and clean up
if (!ms_bFFmpegInstalled) { return; }
if (m_pFILE) { _pclose(m_pFILE); m_pFILE = nullptr; }
if (m_pStaging) {m_pStaging ->Release(); m_pStaging = nullptr; }
m_buffer[0].swap(std::vector<uint8_t>());
m_buffer[1].swap(std::vector<uint8_t>());
m_buffer[2].swap(std::vector<uint8_t>());
m_timestamp[0].QuadPart = m_timestamp[1].QuadPart = 0;
m_frequency.QuadPart = 0;
m_startTime.QuadPart = 0;
m_width = -1;
m_height = -1;
m_frame = -1;
}
HRESULT FFmpeg::AddFrame(ID3D11DeviceContext* pd3dContext, ID3D11RenderTargetView* pRenderTargetView)
{
if (!ms_bFFmpegInstalled) { return S_OK; }
HRESULT hr;
// 1) Copy render target to staging texture
ID3D11Resource* pTex2D = nullptr;
pRenderTargetView->GetResource(&pTex2D);
pd3dContext->CopyResource(m_pStaging, pTex2D);
pTex2D->Release();
// 2) Download staging texture from GPU to buffer
m_buffer[0].swap(m_buffer[1]);
uint32_t* pixels = (uint32_t*)(m_buffer[1].data());
D3D11_MAPPED_SUBRESOURCE mapped;
hr = pd3dContext->Map(m_pStaging, 0, D3D11_MAP_READ, 0, &mapped);
if (hr == S_OK)
{
for (int y = 0; y < m_height; y++)
{
uint32_t* pRowData = (uint32_t*)mapped.pData + y * (mapped.RowPitch / sizeof(uint32_t));
for (int x = 0; x < m_width; x++)
{
pixels[y * m_width + x] = pRowData[x];
}
}
pd3dContext->Unmap(m_pStaging, 0);
}
// 3) Query timestamps and write first frame to ffmpeg
if (m_frame == -1)
{
QueryPerformanceFrequency(&m_frequency);
QueryPerformanceCounter(&m_startTime);
m_timestamp[0].QuadPart = m_timestamp[1].QuadPart = 0;
fwrite(m_buffer[1].data(), 1, m_buffer[1].size(), m_pFILE);
m_frame ++;
}
else
{
m_timestamp[0] = m_timestamp[1];
QueryPerformanceCounter(&m_timestamp[1]);
m_timestamp[1].QuadPart -= m_startTime.QuadPart;
}
// 4) Write frame(s) to ffmpeg depending on recording mode
if (m_mode == MODE_FRAME_COPY)
{
fwrite(m_buffer[1].data(), 1, m_buffer[1].size(), m_pFILE);
}
else
{
while (double(m_frame + 1) * m_frequency.QuadPart / m_fps <= m_timestamp[1].QuadPart)
{
if (m_mode == MODE_INTERPOLATE)
{
double t = (double(m_frame + 1) * m_frequency.QuadPart / m_fps - m_timestamp[0].QuadPart)
/ (m_timestamp[1].QuadPart - m_timestamp[0].QuadPart);
// interpolate
for (size_t i = 0; i < m_buffer[0].size(); i++)
{
m_buffer[2][i] = uint8_t((1 - t) * m_buffer[0][i] + t * m_buffer[1][i]);
}
fwrite(m_buffer[2].data(), 1, m_buffer[2].size(), m_pFILE);
}
else if (m_mode == MODE_DUPLICATE)
{
fwrite(m_buffer[1].data(), 1, m_buffer[1].size(), m_pFILE);
}
m_frame ++;
}
}
return hr;
}

67
Simulations/util/FFmpeg.h Normal file
View File

@@ -0,0 +1,67 @@
#ifndef __FFmpeg_h__
#define __FFmpeg_h__
#include <stdio.h>
#include <cstdint>
#include <vector>
#include <Windows.h>
struct ID3D11Device;
struct ID3D11DeviceContext;
struct ID3D11RenderTargetView;
struct ID3D11Texture2D;
// Simple FFmpeg-based video recorder (http://www.ffmpeg.org) for directly
// writing out h264 encoded videos.
// Only works if "ffmpeg.exe" can be found (e.g. in system path),
// else all calls will return immediately and this class won't do anything.
// FFmpeg windows binaries can be downloaded here: http://ffmpeg.zeranoe.com/builds/
class FFmpeg
{
public:
// FFmpeg Recording mode
enum MODE
{
MODE_FRAME_COPY, // Every added frame is copied to a single video frame
MODE_DUPLICATE, // Realtime recording: duplicate and skip added frames as necessary (faster than MODE_INTERPOLATE)
MODE_INTERPOLATE, // Realtime recording: interpolate and skip added frames as necessary
};
// Create FFmpeg video recorder
// fps: fps of output video
// crf: constant rate factor
// The range of the quantizer scale is 0-51: where 0 is lossless, 23 is codec default,
// and 51 is worst possible. A lower value is a higher quality and a subjectively
// sane range is 18-28. Consider 18 to be visually lossless or nearly so.
// mode: recording mode
FFmpeg(int fps = 24, int crf = 21, MODE mode = MODE_FRAME_COPY);
~FFmpeg();
// Start recording to 'filename' by spawning a ffmpeg subprocess.
// Render target view needed to query information about render target (like e.g. resolution).
HRESULT StartRecording(ID3D11Device* pd3dDevice, ID3D11RenderTargetView* pRenderTargetView, char* filename);
// Add frame from given render target view
HRESULT AddFrame(ID3D11DeviceContext* pd3dContext, ID3D11RenderTargetView* pRenderTargetView);
// Stop recording and stop ffmpeg subprocess
void StopRecording();
private:
FILE* m_pFILE;
ID3D11Texture2D* m_pStaging;
std::vector<uint8_t> m_buffer[3];
LARGE_INTEGER m_timestamp[2];
LARGE_INTEGER m_frequency;
LARGE_INTEGER m_startTime;
int m_width;
int m_height;
MODE m_mode;
int m_fps;
int m_crf;
int m_frame;
static bool ms_bFFmpegInstalled;
};
#endif

901
Simulations/util/matrixbase.h Normal file
View File

@@ -0,0 +1,901 @@
/******************************************************************************
*
* Basic simple matrix class
*
*****************************************************************************/
#include "vectorbase.h"
#include <DirectXMath.h>
#ifndef MATRICES_H
namespace GamePhysics {
// a basic simple 4x4 matrix class
template<class Scalar>
class matrix4x4
{
public:
// Constructor
inline matrix4x4(void );
// Copy-Constructor
inline matrix4x4(const matrix4x4<Scalar> &v );
// DirectX
inline matrix4x4(DirectX::XMMATRIX &m);
// construct a matrix from one Scalar
inline matrix4x4(Scalar);
// construct a matrix from three Scalars
inline matrix4x4(Scalar, Scalar, Scalar);
// construct a matrix from three Scalars
inline matrix4x4(Scalar, Scalar, Scalar,Scalar,
Scalar, Scalar, Scalar,Scalar,
Scalar, Scalar, Scalar,Scalar,
Scalar, Scalar, Scalar,Scalar);
// Assignment operator
inline const matrix4x4<Scalar>& operator= (const matrix4x4<Scalar>& v);
// Assignment operator
inline const matrix4x4<Scalar>& operator= (Scalar s);
// Assign and add operator
inline const matrix4x4<Scalar>& operator+= (const matrix4x4<Scalar>& v);
// Assign and add operator
inline const matrix4x4<Scalar>& operator+= (Scalar s);
// Assign and sub operator
inline const matrix4x4<Scalar>& operator-= (const matrix4x4<Scalar>& v);
// Assign and sub operator
inline const matrix4x4<Scalar>& operator-= (Scalar s);
// Assign and mult operator
inline const matrix4x4<Scalar>& operator*= (const matrix4x4<Scalar>& v);
// Assign and mult operator
inline const matrix4x4<Scalar>& operator*= (Scalar s);
// Assign and div operator
inline const matrix4x4<Scalar>& operator/= (const matrix4x4<Scalar>& v);
// Assign and div operator
inline const matrix4x4<Scalar>& operator/= (Scalar s);
// unary operator
inline matrix4x4<Scalar> operator- () const;
// binary operator add
inline matrix4x4<Scalar> operator+ (const matrix4x4<Scalar>&) const;
// binary operator add
inline matrix4x4<Scalar> operator+ (Scalar) const;
// binary operator sub
inline matrix4x4<Scalar> operator- (const matrix4x4<Scalar>&) const;
// binary operator sub
inline matrix4x4<Scalar> operator- (Scalar) const;
// binary operator mult
inline matrix4x4<Scalar> operator* (const matrix4x4<Scalar>&) const;
// binary operator mult
inline vector3Dim<Scalar> operator* (const vector3Dim<Scalar>&) const;
// binary operator mult
inline matrix4x4<Scalar> operator* (Scalar) const;
// binary operator div
inline matrix4x4<Scalar> operator/ (Scalar) const;
// init function
//! init identity matrix
inline void initId();
//! init rotation matrix
inline void initTranslation(Scalar x, Scalar y, Scalar z);
//! init rotation matrix
inline void initRotationX(Scalar rot);
inline void initRotationY(Scalar rot);
inline void initRotationZ(Scalar rot);
inline void initRotationXYZ(Scalar rotx,Scalar roty, Scalar rotz);
//! init scaling matrix
inline void initScaling(Scalar scale);
inline void initScaling(Scalar x, Scalar y, Scalar z);
inline Vec3 transformVectorNormal(Vec3 v);
inline Vec3 transformVector(Vec3 v);
inline DirectX::XMMATRIX toDirectXMatrix();
inline matrix4x4<Scalar> matrix4x4<Scalar>::inverse();
//! from 16 value array (init id if all 0)
inline void initFromArray(Scalar *array);
inline void transpose();
//! decompose matrix intro translation, scale, rot and shear
void decompose(vector3Dim<Scalar> &trans,vector3Dim<Scalar> &scale,vector3Dim<Scalar> &rot,vector3Dim<Scalar> &shear);
//! public to avoid [][] operators
Scalar value[4][4]; //< Storage of maxtrix values
protected:
};
//------------------------------------------------------------------------------
// STREAM FUNCTIONS
//------------------------------------------------------------------------------
/*************************************************************************
Outputs the object in human readable form using the format
*/
template<class Scalar>
std::ostream&
operator<<( std::ostream& os, const matrix4x4<Scalar>& m )
{
for(int i=0; i<4; i++) {
os << '<' << m.value[0][i] << "," << m.value[1][i] <<"," << m.value[2][i] << "," << m.value[3][i] << '>'<<std::endl;
}
return os;
}
/*************************************************************************
Reads the contents of the object from a stream using the same format
as the output operator.
*/
template<class Scalar>
std::istream&
operator>>( std::istream& is, matrix4x4<Scalar>& m )
{
char c;
char dummy[3];
for(int i=0; i<4; i++) {
is >> c >> m.value[i][0] >> dummy >> m.value[i][1] >> dummy >> m.value[i][2] >> dummy >> m.value[i][3] >> c;
}
return is;
}
//------------------------------------------------------------------------------
// matrix inline FUNCTIONS
//------------------------------------------------------------------------------
/*************************************************************************
Constructor.
*/
template<class Scalar>
inline matrix4x4<Scalar>::matrix4x4( void )
{
for(int i=0; i<4; i++) {
for(int j=0; j<4; j++) {
value[i][j] = 0.0;
}
}
}
/*************************************************************************
Copy-Constructor.
*/
template<class Scalar>
inline matrix4x4<Scalar>::matrix4x4( const matrix4x4<Scalar> &v )
{
value[0][0] = v.value[0][0]; value[0][1] = v.value[0][1]; value[0][2] = v.value[0][2]; value[0][3] = v.value[0][3];
value[1][0] = v.value[1][0]; value[1][1] = v.value[1][1]; value[1][2] = v.value[1][2]; value[1][3] = v.value[1][3];
value[2][0] = v.value[2][0]; value[2][1] = v.value[2][1]; value[2][2] = v.value[2][2]; value[2][3] = v.value[2][3];
value[3][0] = v.value[3][0]; value[3][1] = v.value[3][1]; value[3][2] = v.value[3][2]; value[3][3] = v.value[3][3];
}
template<class Scalar>
inline matrix4x4<Scalar>::matrix4x4(DirectX::XMMATRIX &m )
{
for(int i =0;i<4;i++)
{
value[i][0] = XMVectorGetX(m.r[i]); value[i][1] = XMVectorGetY(m.r[i]); value[i][2] = XMVectorGetZ(m.r[i]); value[i][3] = XMVectorGetW(m.r[i]);
}
}
/*************************************************************************
Constructor for a vector from a single Scalar. All components of
the vector get the same value.
\param s The value to set
\return The new vector
*/
template<class Scalar>
inline matrix4x4<Scalar>::matrix4x4(Scalar s )
{
for(int i=0; i<4; i++) {
for(int j=0; j<4; j++) {
value[i][j] = s;
}
}
}
/*************************************************************************
Constructor for a vector from a single Scalar. All components of
the vector get the same value.
\param s The value to set
\return The new vector
*/
template<class Scalar>
inline matrix4x4<Scalar>::matrix4x4(Scalar s1,Scalar s2,Scalar s3 )
{
for(int i=0; i<4; i++) {
value[i][0] = s1;
value[i][1] = s2;
value[i][2] = s3;
value[i][3] = 0;
}
}
template<class Scalar>
inline matrix4x4<Scalar>::matrix4x4(Scalar s00,Scalar s01,Scalar s02,Scalar s03,
Scalar s10,Scalar s11,Scalar s12,Scalar s13,
Scalar s20,Scalar s21,Scalar s22,Scalar s23,
Scalar s30,Scalar s31,Scalar s32,Scalar s33)
{
value[0][0] = s00;
value[0][1] = s01;
value[0][2] = s02;
value[0][3] = s03;
value[1][0] = s10;
value[1][1] = s11;
value[1][2] = s12;
value[1][3] = s13;
value[2][0] = s20;
value[2][1] = s21;
value[2][2] = s22;
value[2][3] = s23;
value[3][0] = s30;
value[3][1] = s31;
value[3][2] = s32;
value[3][3] = s33;
}
/*************************************************************************
Copy a matrix4x4 componentwise.
\param v vector with values to be copied
\return Reference to self
*/
template<class Scalar>
inline const matrix4x4<Scalar>&
matrix4x4<Scalar>::operator=( const matrix4x4<Scalar> &v )
{
value[0][0] = v.value[0][0]; value[0][1] = v.value[0][1]; value[0][2] = v.value[0][2]; value[0][3] = v.value[0][3];
value[1][0] = v.value[1][0]; value[1][1] = v.value[1][1]; value[1][2] = v.value[1][2]; value[1][3] = v.value[1][3];
value[2][0] = v.value[2][0]; value[2][1] = v.value[2][1]; value[2][2] = v.value[2][2]; value[2][3] = v.value[2][3];
value[3][0] = v.value[3][0]; value[3][1] = v.value[3][1]; value[3][2] = v.value[3][2]; value[3][3] = v.value[3][3];
return *this;
}
/*************************************************************************
Copy a Scalar to each component.
\param s The value to copy
\return Reference to self
*/
template<class Scalar>
inline const matrix4x4<Scalar>&
matrix4x4<Scalar>::operator=(Scalar s)
{
for(int i=0; i<4; i++) {
for(int j=0; j<4; j++) {
value[i][j] = s;
}
}
return *this;
}
/*************************************************************************
Add another matrix4x4 componentwise.
\param v vector with values to be added
\return Reference to self
*/
template<class Scalar>
inline const matrix4x4<Scalar>&
matrix4x4<Scalar>::operator+=( const matrix4x4<Scalar> &v )
{
value[0][0] += v.value[0][0]; value[0][1] += v.value[0][1]; value[0][2] += v.value[0][2]; value[0][3] += v.value[0][3];
value[1][0] += v.value[1][0]; value[1][1] += v.value[1][1]; value[1][2] += v.value[1][2]; value[1][3] += v.value[1][3];
value[2][0] += v.value[2][0]; value[2][1] += v.value[2][1]; value[2][2] += v.value[2][2]; value[2][3] += v.value[2][3];
value[3][0] += v.value[3][0]; value[3][1] += v.value[3][1]; value[3][2] += v.value[3][2]; value[3][3] += v.value[3][3];
return *this;
}
/*************************************************************************
Add a Scalar value to each component.
\param s Value to add
\return Reference to self
*/
template<class Scalar>
inline const matrix4x4<Scalar>&
matrix4x4<Scalar>::operator+=(Scalar s)
{
for(int i=0; i<4; i++) {
for(int j=0; j<4; j++) {
value[i][j] += s;
}
}
return *this;
}
/*************************************************************************
Subtract another vector componentwise.
\param v vector of values to subtract
\return Reference to self
*/
template<class Scalar>
inline const matrix4x4<Scalar>&
matrix4x4<Scalar>::operator-=( const matrix4x4<Scalar> &v )
{
value[0][0] -= v.value[0][0]; value[0][1] -= v.value[0][1]; value[0][2] -= v.value[0][2]; value[0][3] -= v.value[0][3];
value[1][0] -= v.value[1][0]; value[1][1] -= v.value[1][1]; value[1][2] -= v.value[1][2]; value[1][3] -= v.value[1][3];
value[2][0] -= v.value[2][0]; value[2][1] -= v.value[2][1]; value[2][2] -= v.value[2][2]; value[2][3] -= v.value[2][3];
value[3][0] -= v.value[3][0]; value[3][1] -= v.value[3][1]; value[3][2] -= v.value[3][2]; value[3][3] -= v.value[3][3];
return *this;
}
/*************************************************************************
Subtract a Scalar value from each component.
\param s Value to subtract
\return Reference to self
*/
template<class Scalar>
inline const matrix4x4<Scalar>&
matrix4x4<Scalar>::operator-=(Scalar s)
{
for(int i=0; i<4; i++) {
for(int j=0; j<4; j++) {
value[i][j] -= s;
}
}
return *this;
}
/*************************************************************************
Multiply with another vector componentwise.
\param v vector of values to multiply with
\return Reference to self
*/
template<class Scalar>
inline const matrix4x4<Scalar>&
matrix4x4<Scalar>::operator*=( const matrix4x4<Scalar> &v )
{
matrix4x4<Scalar> nv(0.0);
for(int i=0; i<4; i++) {
for(int j=0; j<4; j++) {
for(int k=0;k<4;k++)
nv.value[i][j] += (value[i][k] * v.value[k][j]);
}
}
*this = nv;
return *this;
}
/*************************************************************************
Multiply each component with a Scalar value.
\param s Value to multiply with
\return Reference to self
*/
template<class Scalar>
inline const matrix4x4<Scalar>&
matrix4x4<Scalar>::operator*=(Scalar s)
{
for(int i=0; i<4; i++) {
for(int j=0; j<4; j++) {
value[i][j] *= s;
}
}
return *this;
}
/*************************************************************************
Divide each component by a Scalar value.
\param s Value to divide by
\return Reference to self
*/
template<class Scalar>
inline const matrix4x4<Scalar>&
matrix4x4<Scalar>::operator/=(Scalar s)
{
for(int i=0; i<4; i++) {
for(int j=0; j<4; j++) {
value[i][j] /= s;
}
}
return *this;
}
//------------------------------------------------------------------------------
// unary operators
//------------------------------------------------------------------------------
/*************************************************************************
Build componentwise the negative this vector.
\return The new (negative) vector
*/
template<class Scalar>
inline matrix4x4<Scalar>
matrix4x4<Scalar>::operator-() const
{
matrix4x4<Scalar> nv;
for(int i=0; i<4; i++) {
for(int j=0; j<4; j++) {
nv.value[i][j] = -value[i][j];
}
}
return nv;
}
//------------------------------------------------------------------------------
// binary operators
//------------------------------------------------------------------------------
/*************************************************************************
Build a vector with another vector added componentwise.
\param v The second vector to add
\return The sum vector
*/
template<class Scalar>
inline matrix4x4<Scalar>
matrix4x4<Scalar>::operator+( const matrix4x4<Scalar> &v ) const
{
matrix4x4<Scalar> nv;
for(int i=0; i<4; i++) {
for(int j=0; j<4; j++) {
nv.value[i][j] = value[i][j] + v.value[i][j];
}
}
return nv;
}
/*************************************************************************
Build a vector with a Scalar value added to each component.
\param s The Scalar value to add
\return The sum vector
*/
template<class Scalar>
inline matrix4x4<Scalar>
matrix4x4<Scalar>::operator+(Scalar s) const
{
matrix4x4<Scalar> nv;
for(int i=0; i<4; i++) {
for(int j=0; j<4; j++) {
nv.value[i][j] = value[i][j] + s;
}
}
return nv;
}
/*************************************************************************
Build a vector with another vector subtracted componentwise.
\param v The second vector to subtract
\return The difference vector
*/
template<class Scalar>
inline matrix4x4<Scalar>
matrix4x4<Scalar>::operator-( const matrix4x4<Scalar> &v ) const
{
matrix4x4<Scalar> nv;
for(int i=0; i<4; i++) {
for(int j=0; j<4; j++) {
nv.value[i][j] = value[i][j] - v.value[i][j];
}
}
return nv;
}
/*************************************************************************
Build a vector with a Scalar value subtracted componentwise.
\param s The Scalar value to subtract
\return The difference vector
*/
template<class Scalar>
inline matrix4x4<Scalar>
matrix4x4<Scalar>::operator-(Scalar s ) const
{
matrix4x4<Scalar> nv;
for(int i=0; i<4; i++) {
for(int j=0; j<4; j++) {
nv.value[i][j] = value[i][j] - s;
}
}
return nv;
}
/*************************************************************************
Build a matrix4x4 with a Scalar value multiplied to each component.
\param s The Scalar value to multiply with
\return The product vector
*/
template<class Scalar>
inline matrix4x4<Scalar>
matrix4x4<Scalar>::operator*(Scalar s) const
{
matrix4x4<Scalar> nv;
for(int i=0; i<4; i++) {
for(int j=0; j<4; j++) {
nv.value[i][j] = ((Real)value[i][j]) * s;
}
}
return nv;
}
/*************************************************************************
Build a vector divided componentwise by a Scalar value.
\param s The Scalar value to divide by
\return The ratio vector
*/
template<class Scalar>
inline matrix4x4<Scalar>
matrix4x4<Scalar>::operator/(Scalar s) const
{
matrix4x4<Scalar> nv;
for(int i=0; i<4; i++) {
for(int j=0; j<4; j++) {
nv.value[i][j] = value[i][j] / s;
}
}
return nv;
}
/*************************************************************************
Build a vector with another vector multiplied by componentwise.
\param v The second vector to muliply with
\return The product vector
*/
template<class Scalar>
inline matrix4x4<Scalar>
matrix4x4<Scalar>::operator*( const matrix4x4<Scalar>& v) const
{
matrix4x4<Scalar> nv(0.0);
for(int i=0; i<4; i++) {
for(int j=0; j<4; j++) {
for(int k=0;k<4;k++)
nv.value[i][j] += (value[i][k] * v.value[k][j]);
}
}
return nv;
}
template<class Scalar>
inline vector3Dim<Scalar>
matrix4x4<Scalar>::operator*( const vector3Dim<Scalar>& v) const
{
vector3Dim<Scalar> nvec(0.0);
for(int i=0; i<3; i++) {
for(int j=0; j<3; j++) {
nvec[i] += (v[j] * value[i][j]);
}
}
// assume normalized w coord
for(int i=0; i<3; i++) {
nvec[i] += (1.0 * value[i][3]);
}
return nvec;
}
//------------------------------------------------------------------------------
// Other helper functions
//------------------------------------------------------------------------------
//! init identity matrix
template<class Scalar>
inline void matrix4x4<Scalar>::initId()
{
(*this) = (Scalar)(0.0);
value[0][0] =
value[1][1] =
value[2][2] =
value[3][3] = (Scalar)(1.0);
}
//! init rotation matrix
template<class Scalar>
inline void matrix4x4<Scalar>::initTranslation(Scalar x, Scalar y, Scalar z)
{
//(*this) = (Scalar)(0.0);
this->initId();
value[3][0] = x;
value[3][1] = y;
value[3][2] = z;
}
//! init rotation matrix
template<class Scalar>
inline void
matrix4x4<Scalar>::initRotationX(Scalar rot)
{
double drot = (double)((rot/180)*M_PI);
//? while(drot < 0.0) drot += (M_PI*2.0);
this->initId();
value[1][1] = (Scalar) cos(drot);
value[2][1] = (Scalar) sin(drot);
value[1][2] = (Scalar)(-sin(drot));
value[2][2] = (Scalar) cos(drot);
}
template<class Scalar>
inline void
matrix4x4<Scalar>::initRotationY(Scalar rot)
{
double drot = (double)(rot/360.0*2.0*M_PI);
//? while(drot < 0.0) drot += (M_PI*2.0);
this->initId();
value[0][0] = (Scalar) cos(drot);
value[2][0] = (Scalar)(-sin(drot));
value[0][2] = (Scalar) sin(drot);
value[2][2] = (Scalar) cos(drot);
}
template<class Scalar>
inline void
matrix4x4<Scalar>::initRotationZ(Scalar rot)
{
double drot = (double)(rot/360.0*2.0*M_PI);
//? while(drot < 0.0) drot += (M_PI*2.0);
this->initId();
value[0][0] = (Scalar) cos(drot);
value[1][0] = (Scalar) sin(drot);
value[0][1] = (Scalar)(-sin(drot));
value[1][1] = (Scalar) cos(drot);
}
template<class Scalar>
inline void
matrix4x4<Scalar>::initRotationXYZ( Scalar rotx, Scalar roty, Scalar rotz)
{
matrix4x4<Scalar> val;
matrix4x4<Scalar> rot;
this->initId();
// org
/*rot.initRotationX(rotx);
(*this) *= rot;
rot.initRotationY(roty);
(*this) *= rot;
rot.initRotationZ(rotz);
(*this) *= rot;
// org */
// blender
rot.initRotationZ(rotz);
(*this) *= rot;
rot.initRotationY(roty);
(*this) *= rot;
rot.initRotationX(rotx);
(*this) *= rot;
// blender */
}
//! transpose matrix
template<class Scalar>
inline void
matrix4x4<Scalar>::transpose()
{
for (int i=0;i<4;i++)
for (int j=i+1;j<4;j++)
{
Scalar a=value[i][j];
value[i][j]=value[j][i];
value[j][i]=a;
}
}
//! init scaling matrix
template<class Scalar>
inline void
matrix4x4<Scalar>::initScaling(Scalar scale)
{
this->initId();
value[0][0] = scale;
value[1][1] = scale;
value[2][2] = scale;
}
//! init scaling matrix
template<class Scalar>
inline void
matrix4x4<Scalar>::initScaling(Scalar x, Scalar y, Scalar z)
{
this->initId();
value[0][0] = x;
value[1][1] = y;
value[2][2] = z;
}
// I will Think About cleaner way
template<class Scalar>
inline Vec3
matrix4x4<Scalar>::transformVector(Vec3 v)
{
DirectX::XMVECTOR in = v.toDirectXVector();
DirectX::XMVECTOR out = DirectX::XMVector3Transform(in,this->toDirectXMatrix());
Vec3 result = Vec3(out);
return result;
}
// I will Think About cleaner way
template<class Scalar>
inline Vec3
matrix4x4<Scalar>::transformVectorNormal(Vec3 v)
{
DirectX::XMVECTOR in = v.toDirectXVector();
DirectX::XMVECTOR out = DirectX::XMVector3TransformNormal(in,this->toDirectXMatrix());
Vec3 result = Vec3(out);
return result;
}
// I will Think About cleaner way
template<class Scalar>
inline matrix4x4<Scalar>
matrix4x4<Scalar>::inverse()
{
DirectX::XMMATRIX out = DirectX::XMMatrixInverse(nullptr,this->toDirectXMatrix());
Mat4 m(out);
return m;
}
template<class Scalar>
inline DirectX::XMMATRIX
matrix4x4<Scalar>::toDirectXMatrix()
{
XMMATRIX m = XMMatrixSet(this->value[0][0],this->value[0][1],this->value[0][2],this->value[0][3],
this->value[1][0],this->value[1][1],this->value[1][2],this->value[1][3],
this->value[2][0],this->value[2][1],this->value[2][2],this->value[2][3],
this->value[3][0],this->value[3][1],this->value[3][2],this->value[3][3]);
return m;
}
//! from 16 value array (init id if all 0)
template<class Scalar>
inline void
matrix4x4<Scalar>::initFromArray(Scalar *array)
{
bool allZero = true;
for(int i=0; i<4; i++) {
for(int j=0; j<4; j++) {
value[j][i] = array[i*4+j];
if(array[i*4+j]!=0.0) allZero=false;
}
}
if(allZero) this->initId();
}
//! decompose matrix
template<class Scalar>
void
matrix4x4<Scalar>::decompose(vector3Dim<Scalar> &trans,vector3Dim<Scalar> &scale,vector3Dim<Scalar> &rot,vector3Dim<Scalar> &shear) {
Vec3 row[3],temp;
for(int i = 0; i < 3; i++) {
trans[i] = this->value[3][i];
}
for(int i = 0; i < 3; i++) {
row[i][0] = this->value[i][0];
row[i][1] = this->value[i][1];
row[i][2] = this->value[i][2];
}
scale[0] = norm(row[0]);
normalize (row[0]);
shear[0] = dot(row[0], row[1]);
row[1][0] = row[1][0] - shear[0]*row[0][0];
row[1][1] = row[1][1] - shear[0]*row[0][1];
row[1][2] = row[1][2] - shear[0]*row[0][2];
scale[1] = norm(row[1]);
normalize (row[1]);
if(scale[1] != 0.0)
shear[0] /= scale[1];
shear[1] = dot(row[0], row[2]);
row[2][0] = row[2][0] - shear[1]*row[0][0];
row[2][1] = row[2][1] - shear[1]*row[0][1];
row[2][2] = row[2][2] - shear[1]*row[0][2];
shear[2] = dot(row[1], row[2]);
row[2][0] = row[2][0] - shear[2]*row[1][0];
row[2][1] = row[2][1] - shear[2]*row[1][1];
row[2][2] = row[2][2] - shear[2]*row[1][2];
scale[2] = norm(row[2]);
normalize (row[2]);
if(scale[2] != 0.0) {
shear[1] /= scale[2];
shear[2] /= scale[2];
}
temp = cross(row[1], row[2]);
if(dot(row[0], temp) < 0.0) {
for(int i = 0; i < 3; i++) {
scale[i] *= -1.0;
row[i][0] *= -1.0;
row[i][1] *= -1.0;
row[i][2] *= -1.0;
}
}
if(row[0][2] < -1.0) row[0][2] = -1.0;
if(row[0][2] > +1.0) row[0][2] = +1.0;
rot[1] = asin(-row[0][2]);
if(fabs(cos(rot[1])) > VECTOR_EPSILON) {
rot[0] = atan2 (row[1][2], row[2][2]);
rot[2] = atan2 (row[0][1], row[0][0]);
}
else {
rot[0] = atan2 (row[1][0], row[1][1]);
rot[2] = 0.0;
}
rot[0] = (180.0/M_PI)*rot[0];
rot[1] = (180.0/M_PI)*rot[1];
rot[2] = (180.0/M_PI)*rot[2];
}
//------------------------------------------------------------------------------
// TYPEDEFS
//------------------------------------------------------------------------------
typedef matrix4x4<double> Mat4d;
// a 3D vector with single precision
typedef matrix4x4<float> Mat4f;
// a 3D integer vector
typedef matrix4x4<int> Mat4i;
// default vector typing
// a 3D vector for graphics output, typically float?
typedef matrix4x4<Real> Mat4;
}; // namespace DDF
#define MATRICES_H
#endif

173
Simulations/util/quaternion.h Normal file
View File

@@ -0,0 +1,173 @@
/******************************************************************************
*
* Basic simple Quaternion class
*
*****************************************************************************/
#ifndef UTIL_QUATERIONS_H
#define UTIL_QUATERIONS_H
#include "matrixbase.h"
#include "vectorbase.h"
#define smax(a,b) ((a>b)?(a):(b))
#define scpysign(a,b) ((a)*(b)>=0?(a):-(a))
namespace GamePhysics {
template<class Scalar>
class Quaternion
{
public:
Scalar x,y,z,w;
Quaternion () : x(0),y(0),z(0),w(0) {}
Quaternion (Scalar nx, Scalar ny, Scalar nz, Scalar nw) :
x(nx),y(ny),z(nz),w(nw) {}
Quaternion (const Quaternion &q)
{
x=q.x; y=q.y; z=q.z; w=q.w;
}
Quaternion (const matrix4x4<Scalar> &M)
{
w = sqrt( smax( 0, 1 + M.value[0][0] + M.value[1][1] + M.value[2][2] ) ) / 2;
x = sqrt( smax( 0, 1 + M.value[0][0] - M.value[1][1] - M.value[2][2] ) ) / 2;
y = sqrt( smax( 0, 1 - M.value[0][0] + M.value[1][1] - M.value[2][2] ) ) / 2;
z = sqrt( smax( 0, 1 - M.value[0][0] - M.value[1][1] + M.value[2][2] ) ) / 2;
x = scpysign( x, M.value[2][1] - M.value[1][2] );
y = scpysign( y, M.value[0][2] - M.value[2][0] );
z = scpysign( z, M.value[1][0] - M.value[0][1] );
}
Quaternion (const vector3Dim<Scalar>& axis, Scalar angle)
{
Scalar mult = sin(angle*.5);
x = axis.x * mult;
y = axis.y * mult;
z = axis.z * mult;
w = cos (angle*.5);
}
Quaternion (Scalar rx, Scalar ry, Scalar rz)
{
Quaternion qx(vector3Dim<Scalar> (1.,0.,0.),-rx);
Quaternion qy(vector3Dim<Scalar> (0.,1.,0.),-ry);
Quaternion qz(vector3Dim<Scalar> (0.,0.,1.),-rz);
Quaternion q = qz*qy*qx;
x=q.x; y=q.y; z=q.z; w=q.w;
}
matrix4x4<Scalar> getRotMat() const
{
DirectX::XMVECTOR v = XMVectorSet(x,y,z,w);
matrix4x4<Scalar> M(DirectX::XMMatrixRotationQuaternion(v));
return M;
}
vector3Dim<Scalar> getAxis() const
{
Scalar phi2 = acos(w);
vector3Dim<Scalar> axis = vector3Dim<Scalar> (x,y,z) * (1./sin(phi2));
normalize(axis);
return axis * 2.* phi2;
}
inline const Quaternion operator+=(const Quaternion &q) { x+=q.x; y+=q.y; z+=q.z; w+=q.w; return *this; };
inline const Quaternion operator+=(const Scalar m) { x+=m; y+=m; z+=m; w+=m; return *this; };
inline const Quaternion operator-=(const Quaternion &q) { x-=q.x; y-=q.y; z-=q.z; w-=q.w; return *this; };
inline const Quaternion operator-=(const Scalar m) { x-=m; y-=m; z-=m; w-=m; return *this; };
inline const Quaternion operator*=(const Quaternion &q)
{
vector3Dim<Scalar> v1(x,y,z), v2(q.x,q.y,q.z);
vector3Dim<Scalar> nv = v1*q.w + v2*w + cross(v2,v1);
Scalar nw = w*q.w - (v1.x*v2.x+v1.y*v2.y+v1.z*v2.z);
x = nv.x;
y = nv.y;
z = nv.z;
w = nw;
return *this;
}
inline const Quaternion operator*=(const Scalar m) { x*=m;y*=m;z*=m;w*=m; return *this; };
inline const Quaternion operator/=(const Scalar m) { x/=m;y/=m;z/=m;w/=m; return *this; };
inline const Quaternion operator+(const Quaternion &q) const { return Quaternion(x+q.x,y+q.y,z+q.z,w+q.w); };
inline const Quaternion operator-(const Quaternion &q) const { return Quaternion(x-q.x,y-q.y,z-q.z,w-q.w); };
inline const Quaternion operator*(const Scalar m) const { return Quaternion(x*m,y*m,z*m,w*m); };
inline const Quaternion operator-() const { return Quaternion(-x,-y,-z,-w); };
inline Scalar dot(const Quaternion &q) const { return x*q.x+y*q.y+z*q.z+w*q.w; }
inline Scalar normSq() const { return x*x+y*y+z*z+w*w; }
inline Scalar norm() const { return sqrt(normSq()); }
inline const Quaternion unit() const { Scalar d=1./norm(); return Quaternion(x*d,y*d,z*d,w*d); }
inline const DirectX::XMVECTOR toDirectXQuat(){return XMVectorSet(x,y,z,w);};
inline const Quaternion operator*(const Quaternion &q) const
{
vector3Dim<Scalar> v1(x,y,z), v2(q.x,q.y,q.z);
vector3Dim<Scalar> nv = v1*q.w + v2*w + cross(v2,v1);
Scalar nw = w*q.w - (v1.x*v2.x+v1.y*v2.y+v1.z*v2.z);
return Quaternion(nv.x,nv.y,nv.z,nw);
}
static const Quaternion slerp(Quaternion q, Quaternion p, Scalar t)
{
Scalar cosphi = q.dot(p);
if(cosphi < 0.0f)
{
cosphi *= -1.0f;
q = -q;
}
const Scalar DOT_THRESHOLD = 0.9995f;
if (cosphi > DOT_THRESHOLD) {
// interpolate linearly
return (q + (p - q) * t).unit();
}
Scalar sinphi = sqrt(1. - cosphi * cosphi);
Scalar phi = acos(cosphi);
Quaternion res = q * (sin( phi * (1.-t) ) / sinphi) + p * (sin( phi * t) / sinphi);
return res;
}
};
template<class Scalar>
inline Quaternion<Scalar> operator+(int s, Quaternion<Scalar> &q) { return q + s; }
template<class Scalar>
inline Quaternion<Scalar> operator+(float s, Quaternion<Scalar> &q) { return q + s; }
template<class Scalar>
inline Quaternion<Scalar> operator+(double s, Quaternion<Scalar> &q) { return q + s; }
template<class Scalar>
inline Quaternion<Scalar> operator*(int s, Quaternion<Scalar> &q) { return q * s; }
template<class Scalar>
inline Quaternion<Scalar> operator*(float s, Quaternion<Scalar> &q) { return q * s; }
template<class Scalar>
inline Quaternion<Scalar> operator*(double s, Quaternion<Scalar> &q) { return q * s; }
template<class Scalar>
std::ostream&
operator<<( std::ostream& os, const Quaternion<Scalar>& q)
{
char buf[256];
snprintf(buf,256,"<%g,%g,%g,%g>", (double)q.x,(double)q.y,(double)q.z,(double)q.w);
os << std::string(buf);
return os;
}
typedef Quaternion<Real> Quat;
}
#endif // __Quaternion_h__

65
Simulations/util/timer.h Normal file
View File

@@ -0,0 +1,65 @@
#ifndef TIMER_H
#define TIMER_H
/*
// simple example:
// global declarations
MuTime myTimer;
// start timer function
myTimer.get();
// ...do actual work...
// stop timer function & print
std::cout << "Time passed "<< myTimer.update().time <<" milliseconds\n";
*/
#if defined(WIN32) || defined(_WIN32)
# include <windows.h>
#else
# include <sys/time.h>
#endif
struct MuTime {
MuTime() {}
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; }
void clear() { time = 0; }
void get() {
#if defined(WIN32) || defined(_WIN32)
LARGE_INTEGER liTimerFrequency;
LARGE_INTEGER liLastTime;
QueryPerformanceFrequency(&liTimerFrequency);
QueryPerformanceCounter(&liLastTime);
double timepass = (double)liLastTime.QuadPart;
timepass /= (double)liTimerFrequency.QuadPart;
time = (timepass * 1000);
#else
struct timeval tv;
struct timezone tz;
tz.tz_minuteswest = 0;
tz.tz_dsttime = 0;
gettimeofday(&tv, &tz);
time = (tv.tv_sec * 1000) + (tv.tv_usec / 1000);
#endif
}
MuTime update(){
MuTime o = *this;
get();
return *this - o;
}
unsigned long time;
};
#endif

58
Simulations/util/util.cpp Normal file
View File

@@ -0,0 +1,58 @@
#include "util.h"
#include <Windows.h>
#include <DXUT.h>
std::wstring GetExePath()
{
// get full path to .exe
const size_t bufferSize = 1024;
wchar_t buffer[bufferSize];
if(0 == GetModuleFileNameW(nullptr, buffer, bufferSize))
{
return std::wstring(L"");
}
std::wstring path(buffer);
// extract path (remove filename)
size_t posSlash = path.find_last_of(L"/\\");
if(posSlash != std::wstring::npos)
{
path = path.substr(0, posSlash + 1);
}
return path;
}
void UpdateWindowTitle(const std::wstring& appName)
{
// check if we should update the window title
bool update = false;
// update if window size changed
static int s_windowWidth = 0;
static int s_windowHeight = 0;
if (s_windowWidth != DXUTGetWindowWidth() || s_windowHeight != DXUTGetWindowHeight()) {
s_windowWidth = DXUTGetWindowWidth();
s_windowHeight = DXUTGetWindowHeight();
update = true;
}
// update if fps changed (updated once per second by DXUT)
static float s_fps = 0.0f;
static float s_mspf = 0.0f;
if (s_fps != DXUTGetFPS()) {
s_fps = DXUTGetFPS();
s_mspf = 1000.0f / s_fps;
update = true;
}
// update window title if something relevant changed
if (update) {
const size_t len = 512;
wchar_t str[len];
swprintf_s(str, len, L"%s %ux%u @ %.2f fps / %.2f ms", appName.c_str(), s_windowWidth, s_windowHeight, s_fps, s_mspf);
SetWindowText(DXUTGetHWND(), str);
}
}

13
Simulations/util/util.h Normal file
View File

@@ -0,0 +1,13 @@
#ifndef __util_h__
#define __util_h__
#include <string>
std::wstring GetExePath();
void UpdateWindowTitle(const std::wstring& appName);
#endif

463
Simulations/util/vector4d.h Normal file
View File

@@ -0,0 +1,463 @@
/******************************************************************************
*
* Basic 4d vector class
*
*****************************************************************************/
#ifndef GamePhysics_BASICVEC4D_H
#define GamePhysics_BASICVEC4D_H
#include "vectorbase.h"
namespace GamePhysics
{
// basic inlined vector class
template<class Scalar>
class ntlVector4Dim
{
public:
//! Constructor
inline ntlVector4Dim() : x(0),y(0),z(0),t(0) {}
//! Copy-Constructor
inline ntlVector4Dim ( const ntlVector4Dim<Scalar> &v ) : x(v.x), y(v.y), z(v.z),t(v.t) {}
//! Copy-Constructor
inline ntlVector4Dim ( const float * v) : x((Scalar)v[0]), y((Scalar)v[1]), z((Scalar)v[2]), t((Scalar)v[3]) {}
//! Copy-Constructor
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 ntlVector4Dim ( Scalar v) : x(v), y(v), z(v), t(v) {}
//! Construct a vector from four Ss
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 ntlVector4Dim(DirectX::XMVECTOR &v ); // TODO CHECK!
// get address of array for OpenGL
Scalar *getAddress() { return value; }
// Operators
//! Assignment operator
inline const ntlVector4Dim<Scalar>& operator= ( const ntlVector4Dim<Scalar>& v ) {
x = v.x;
y = v.y;
z = v.z;
t = v.t;
return *this;
}
//! Assignment operator
inline const ntlVector4Dim<Scalar>& operator= ( Scalar s ) {
x = y = z = t = s;
return *this;
}
//! Assign and add operator
inline const ntlVector4Dim<Scalar>& operator+= ( const ntlVector4Dim<Scalar>& v ) {
x += v.x;
y += v.y;
z += v.z;
t += v.t;
return *this;
}
//! Assign and add operator
inline const ntlVector4Dim<Scalar>& operator+= ( Scalar s ) {
x += s;
y += s;
z += s;
t += s;
return *this;
}
//! Assign and sub operator
inline const ntlVector4Dim<Scalar>& operator-= ( const ntlVector4Dim<Scalar>& v ) {
x -= v.x;
y -= v.y;
z -= v.z;
t -= v.t;
return *this;
}
//! Assign and sub operator
inline const ntlVector4Dim<Scalar>& operator-= ( Scalar s ) {
x -= s;
y -= s;
z -= s;
t -= s;
return *this;
}
//! Assign and mult operator
inline const ntlVector4Dim<Scalar>& operator*= ( const ntlVector4Dim<Scalar>& v ) {
x *= v.x;
y *= v.y;
z *= v.z;
t *= v.t;
return *this;
}
//! Assign and mult operator
inline const ntlVector4Dim<Scalar>& operator*= ( Scalar s ) {
x *= s;
y *= s;
z *= s;
t *= s;
return *this;
}
//! Assign and div operator
inline const ntlVector4Dim<Scalar>& operator/= ( const ntlVector4Dim<Scalar>& v ) {
x /= v.x;
y /= v.y;
z /= v.z;
t /= v.t;
return *this;
}
//! Assign and div operator
inline const ntlVector4Dim<Scalar>& operator/= ( Scalar s ) {
x /= s;
y /= s;
z /= s;
t /= s;
return *this;
}
inline void safeDivide (const ntlVector4Dim<Scalar>& v);
//! Negation operator
inline ntlVector4Dim<Scalar> operator- () const {
return ntlVector4Dim<Scalar> (-x, -y, -z, -t);
}
// binary operator add
inline ntlVector4Dim<Scalar> operator+ (const ntlVector4Dim<Scalar>&) const;
// binary operator add
inline ntlVector4Dim<Scalar> operator+ (Scalar) const;
// binary operator sub
inline ntlVector4Dim<Scalar> operator- (const ntlVector4Dim<Scalar>&) const;
// binary operator sub
inline ntlVector4Dim<Scalar> operator- (Scalar) const;
// binary operator mult
inline ntlVector4Dim<Scalar> operator* (const ntlVector4Dim<Scalar>&) const;
// binary operator mult
inline ntlVector4Dim<Scalar> operator* (Scalar) const;
// binary operator div
inline ntlVector4Dim<Scalar> operator/ (const ntlVector4Dim<Scalar>&) const;
// binary operator div
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!!}
//! Get biggest component
//inline Scalar max() const { return ( x>y ) ? ( ( x>z ) ? x:z ) : ( ( y>z ) ? y:z ); // todo t!!}
//! Test if all components are zero
inline bool empty() {
return x==0 && y==0 && z==0 && t==0;
}
//! access operator
inline Scalar& operator[] ( unsigned int i ) {
return value[i];
}
//! constant access operator
inline const Scalar& operator[] ( unsigned int i ) const {
return value[i];
}
//! debug output vector to a string
std::string toString() const;
//! actual values
union {
Scalar value[4];
struct {
Scalar x;
Scalar y;
Scalar z;
Scalar t;
};
struct {
Scalar X;
Scalar Y;
Scalar Z;
Scalar T;
};
};
// zero element
static const ntlVector4Dim<Scalar> ZERO;
protected:
};
//************************************************************************
// binary operators
//************************************************************************
//! Addition operator
template<class Scalar>
inline ntlVector4Dim<Scalar> ntlVector4Dim<Scalar>::operator+ ( const ntlVector4Dim<Scalar> &v) const
{
return ntlVector4Dim<Scalar> (value[0]+v.value[0],
value[1]+v.value[1],
value[2]+v.value[2],
value[3]+v.value[3]);
}
//! Addition operator
template<class Scalar>
inline ntlVector4Dim<Scalar>
ntlVector4Dim<Scalar>::operator+(Scalar s) const
{
return ntlVector4Dim<Scalar>(value[0]+s,
value[1]+s,
value[2]+s,
value[3]+s);
}
template<class Scalar>
inline ntlVector4Dim<Scalar>
operator+(float s, ntlVector4Dim<Scalar> v)
{
return v + s;
}
template<class Scalar>
inline ntlVector4Dim<Scalar>
operator+(double s, ntlVector4Dim<Scalar> v)
{
return v + s;
}
template<class Scalar>
inline ntlVector4Dim<Scalar>
operator+(int s, ntlVector4Dim<Scalar> v)
{
return v + s;
}
//! Subtraction operator
template<class Scalar>
inline ntlVector4Dim<Scalar>
ntlVector4Dim<Scalar>::operator-( const ntlVector4Dim<Scalar> &v ) const
{
return ntlVector4Dim<Scalar>(value[0]-v.value[0],
value[1]-v.value[1],
value[2]-v.value[2],
value[3]-v.value[3]);
}
//! Subtraction operator
template<class Scalar>
inline ntlVector4Dim<Scalar>
ntlVector4Dim<Scalar>::operator-(Scalar s ) const
{
return ntlVector4Dim<Scalar>(value[0]-s,
value[1]-s,
value[2]-s,
value[3]-s,);
}
//! Multiplication operator
template<class Scalar>
inline ntlVector4Dim<Scalar>
ntlVector4Dim<Scalar>::operator* ( const ntlVector4Dim<Scalar>& v ) const
{
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 ntlVector4Dim<Scalar>
ntlVector4Dim<Scalar>::operator* (Scalar s) const
{
return ntlVector4Dim<Scalar>(value[0]*s, value[1]*s, value[2]*s, value[3]*s);
}
//! Multiplication operator
template<class Scalar>
inline ntlVector4Dim<Scalar>
operator* (float s, ntlVector4Dim<Scalar> v)
{
return v * s;
}
template<class Scalar>
inline ntlVector4Dim<Scalar>
operator*(double s, ntlVector4Dim<Scalar> v)
{
return v * s;
}
template<class Scalar>
inline ntlVector4Dim<Scalar>
operator*(int s, ntlVector4Dim<Scalar> v)
{
return v * s;
}
//! Division operator
template<class Scalar>
inline ntlVector4Dim<Scalar>
ntlVector4Dim<Scalar>::operator/ (const ntlVector4Dim<Scalar> & v) const
{
return ntlVector4Dim<Scalar> (value[0]/v.value[0],
value[1]/v.value[1],
value[2]/v.value[2],
value[3]/v.value[3]);
}
//! Division operator
template<class Scalar>
inline ntlVector4Dim<Scalar>
ntlVector4Dim<Scalar>::operator / (Scalar s) const
{
return ntlVector4Dim<Scalar> (value[0]/s,
value[1]/s,
value[2]/s,
value[3]/s);
}
//! Safe divide
template<class Scalar>
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;
value[2] = (v.value[2]!=0) ? (value[2] / v.value[2]) : 0;
value[3] = (v.value[3]!=0) ? (value[3] / v.value[3]) : 0;
}
//************************************************************************
// External functions
//************************************************************************
//! Dot product
template<class Scalar>
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 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 ) ) );
return cp;
}*/
//! Compute the magnitude (length) of the vector
template<class Scalar>
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 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 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 ntlVector4Dim<Scalar> ( v.x*fac, v.y*fac, v.z*fac , v.t*fac );
}
else
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 ( 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 ) {
norm = 1.;
} else if ( l > VECTOR_EPSILON*VECTOR_EPSILON ) {
norm = sqrt ( l );
v *= 1./norm;
} else {
v = ntlVector4Dim<Scalar>::ZERO;
norm = 0.;
}
return ( Scalar ) norm;
}
template<class Scalar>
inline bool equal(const ntlVector4Dim<Scalar> &v, const ntlVector4Dim<Scalar> &c)
{
return (ABS(v[0]-c[0]) +
ABS(v[1]-c[1]) +
ABS(v[2]-c[2]) +
ABS(v[3]-c[3]) < VECTOR_EPSILON);
}
//! Outputs the object in human readable form as string
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 );
}
//! Outputs the object in human readable form to stream
template<class Scalar>
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 );
return os;
}
//! Reads the contents of the object from a stream
template<class Scalar>
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;
return is;
}
/**************************************************************************/
// Define default vector alias
/**************************************************************************/
//! 3D vector class of type Real (typically float)
typedef ntlVector4Dim<Real> Vec4;
// a 3D vector with double precision
typedef ntlVector4Dim<double> nVec4d;
// a 3D vector with single precision
typedef ntlVector4Dim<float> nVec4f;
//! 3D vector class of type int
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]); }
template<class T> inline nVec4i vec42I(T v0, T v1, T v2, T v3) { return nVec4i((int)v0,(int)v1,(int)v2,(int)v3); }
template<class T> inline nVec4d vec42D(T v) { return nVec4d(v[0],v[1],v[2],v[3]); }
template<class T> inline nVec4i vec42D(T v0, T v1, T v2, T v3) { return nVec4d((double)v0,(double)v1,(double)v2,(double)v3); }
template<class T> inline nVec4f vec42F(T v) { return nVec4f(v[0],v[1],v[2],v[3]); }
template<class T> inline nVec4i vec42F(T v0, T v1, T v2, T v3) { return nVec4f((float)v0,(float)v1,(float)v2,(float)v3); }
template<class T> inline nVec4i vec4round(T v) { return nVec4i((int)round(v[0]),(int)round(v[1]),(int)round(v[2]),(int)round(v[3])); }
template<class T> inline Vec4 vec42R(T v) { return Vec4(v[0],v[1],v[2],v[3]); }
}; // namespace
#endif

1260
Simulations/util/vectorbase.h Normal file
View File

File diff suppressed because it is too large Load Diff