void OpenGL::DrawLine(const Matrix& p, const Matrix& q, double width)
{
if ( (p.GetRow() != 3) || (p.GetCol() != 1) || (q.GetRow() != 3) || (q.GetCol() != 1) )
{
fprintf(stderr, "OpenGL::DrawLine Invalid Matrix");
return;
}
DrawLine( p(1), p(2), p(3), q(1), q(2), q(3), width );
return;
}
void OpenGL::DrawFrame(const Matrix& p, const Matrix& q)
{
if ( (p.GetRow() != 3) || (p.GetCol() != 1) || (q.GetRow() != 3) || (q.GetCol() != 1) )
{
fprintf(stderr, "OpenGL::DrawFrame Invalid Matrix");
return;
}
DrawFrame( p(1), p(2), p(3), q(1), q(2), q(3) );
return;
}
void OpenGL::DrawPolygonSurface(const Matrix& p, const Matrix& q, const Matrix& r)
{
if ( (p.GetRow() != 3) || (p.GetCol() != 1) ||
(q.GetRow() != 3) || (q.GetCol() != 1) ||
(r.GetRow() != 3) || (r.GetCol() != 1) )
{
fprintf(stderr, "OpenGL::DrawPolygonSurface Invalid Matrix\n\n");
return;
}
DrawPolygonSurface( p(1), p(2), p(3), q(1), q(2), q(3), r(1), r(2), r(3) );
return;
}
void itr_algorithm::GaussianNaiveBayes::TrainPos(const Matrix &input)
{
int length = input.GetRow();
F32 *data = new F32[length];
F32 mu, sigma;
if (!initpos)
{
for (int i = 0; i < featureNum; i++)
{
input.CopyColTo(i, data);
itr_math::StatisticsObj->Mean(data, length, muPos[i]);
itr_math::StatisticsObj->Variance(data, length, sigmaPos[i]);
}
initpos = true;
}
else
{
for (int i = 0; i < featureNum; i++)
{
input.CopyColTo(i, data);
itr_math::StatisticsObj->Mean(data, length, mu);
itr_math::StatisticsObj->Variance(data, length, sigma);
itr_math::NumericalObj->Sqrt(
LearnRate * sigmaPos[i] * sigmaPos[i] + (1 - LearnRate) * sigma * sigma
+ LearnRate * (1 - LearnRate) * (mu - muPos[i]) * (mu - muPos[i]),
sigmaPos[i]);
muPos[i] = LearnRate * muPos[i] + (1 - LearnRate) * mu;
}
}
}
int Encoder(int res[], Matrix key, int result[], int length) {
int cal_length = key.GetRow();
if (length % cal_length != 0) {
int padding = (length / cal_length + 1) * cal_length - length;
int group_num = length / cal_length + 1;
for (int i = 0; i < padding; i++)
res[length + i] = -1;
for (int k = 0; k < group_num; k++) {
for (int i = 0; i < cal_length; i++) {
int temp_res[cal_length], temp_result[cal_length];
for (int j = 0; j < cal_length; j++)
temp_res[j] = res[j + cal_length * k];
DetailEncoder(temp_res, key, temp_result, cal_length);
for (int j = 0; j < cal_length; j++)
result[cal_length * k + j] = temp_result[j];
}
}
}
else {
int group_num = length / cal_length;
for (int k = 0; k < group_num; k++) {
for (int i = 0; i < cal_length; i++) {
int temp_res[cal_length], temp_result[cal_length];
for (int j = 0; j < cal_length; j++)
temp_res[j] = res[j + cal_length * k];
DetailEncoder(temp_res, key, temp_result, cal_length);
for (int j = 0; j < cal_length; j++)
result[cal_length * k + j] = temp_result[j];
}
}
}
}
//Used for Cascaded Shadow Mapping to calculate shadow cascades.
//This function doesn't calculate frustum's planes
void Camera::SplitFrustum(float zn, float zf, Frustum* pFrustum)
{
Matrix matrix = mOwner->mMatrix * mParentOffset;
Vector xAxis, yAxis, zAxis;
Vector pos = matrix.GetRow(3);
if (projMode == ProjectionMode::Perspective)
{
float y = tanf(perspective.FoV / 2.0f);
float x = perspective.aspectRatio * y;
xAxis = x * matrix.GetRow(0);
yAxis = y * matrix.GetRow(1);
zAxis = matrix.GetRow(2);
pFrustum->verticies[0] = pos + zn * (zAxis - xAxis - yAxis);
pFrustum->verticies[1] = pos + zn * (zAxis + xAxis - yAxis);
pFrustum->verticies[2] = pos + zn * (zAxis - xAxis + yAxis);
pFrustum->verticies[3] = pos + zn * (zAxis + xAxis + yAxis);
pFrustum->verticies[4] = pos + zf * (zAxis - xAxis - yAxis);
pFrustum->verticies[5] = pos + zf * (zAxis + xAxis - yAxis);
pFrustum->verticies[6] = pos + zf * (zAxis - xAxis + yAxis);
pFrustum->verticies[7] = pos + zf * (zAxis + xAxis + yAxis);
}
else
{
xAxis = matrix.GetRow(0);
yAxis = matrix.GetRow(1);
zAxis = matrix.GetRow(2);
pFrustum->verticies[0] = pos + zn * zAxis + ortho.left * xAxis + ortho.bottom * yAxis;
pFrustum->verticies[1] = pos + zn * zAxis + ortho.right * xAxis + ortho.bottom * yAxis;
pFrustum->verticies[2] = pos + zn * zAxis + ortho.left * xAxis + ortho.top * yAxis;
pFrustum->verticies[3] = pos + zn * zAxis + ortho.right * xAxis + ortho.top * yAxis;
pFrustum->verticies[4] = pos + zf * zAxis + ortho.left * xAxis + ortho.bottom * yAxis;
pFrustum->verticies[5] = pos + zf * zAxis + ortho.right * xAxis + ortho.bottom * yAxis;
pFrustum->verticies[6] = pos + zf * zAxis + ortho.left * xAxis + ortho.top * yAxis;
pFrustum->verticies[7] = pos + zf * zAxis + ortho.right * xAxis + ortho.top * yAxis;
}
}
void Draw::Correspond(const Matrix &Img1, const Matrix &Img2,
const vector<Point2D> &feature1, const vector<Point2D> &feature2,S32 FeatureNum,
Matrix &Result)
{
Result.Init(Img1.GetRow() , Img1.GetCol()* 2 + 10);
int offset = Img1.GetCol() + 10;
for (int i = 0; i < Img1.GetCol(); i++)
for (int j = 0; j < Img1.GetRow(); j++)
{
Result(j, i) = Img1(j, i);
Result(j, i + offset) = Img2(j, i);
}
for (int i = 0; i < FeatureNum; i++)
{
Line(Result, feature1[i].X, feature1[i].Y, feature2[i].X + offset, feature2[i].Y,
255);
Circle(Result,feature1[i].X, feature1[i].Y,2,255);
Circle(Result,feature2[i].X + offset, feature2[i].Y,2,255);
}
}
static void printMatrix(Matrix a)
{
for (int i = 0; i < a.GetRow(); ++i)
{
for (int j = 0; j < a.GetCol(); ++j)
{
printf("%f ", a(i, j));
}
printf("\n");
}
printf("\n");
}
// Encoder
int DetailEncoder(int res[], Matrix key, int result[], int length) {
// Initialization
for (int i = 0; i < length; i++)
result[i] = 0;
// Start encoding
for (int i = 0; i < key.GetCol(); i++) {
for (int j = 0; j < key.GetRow(); j++)
result[i] = result[i] + res[j] * key.GetValue(j, i);
}
for (int i = 0; i < length; i++)
result[i] = result[i] % 26;
}
void Matrix::operator =(Matrix& rhs) {
for (int i = 1; i <= 4; i++)
this->SetRow(rhs.GetRow(i), i);
}
void FubiGMR::calculateGMR(std::vector<Fubi::Vec3f>& means, std::vector<Fubi::Matrix3f>& inverseCovs)
{
// Helping vars for extracting the right columns (t, x, y, z)
int outDataSize = means.size();
const int outDim = 3;
Vector inComponents(1), outComponents(outDim);
inComponents(0) = 0;
for (unsigned int i = 0; i < 3; i++)
outComponents(i) = (float)(i + 1);
Matrix inData(outDataSize, 1);
float* inArray = inData.Array();
for (int i = 0; i < outDataSize; ++i)
{
inArray[i] = (float)i;
}
// Calculate probabilites for each data point
Matrix Pxi(outDataSize, m_numStates);
for (int i = 0; i < outDataSize; i++)
{
float norm_f = 0.0f;
Vector row;
for (int s = 0; s < m_numStates; s++)
{
float p_i = m_priors[s] * pdfState(inData.GetRow(i, row), inComponents, s);
Pxi(i, s) = p_i;
norm_f += p_i;
}
Pxi.SetRow(Pxi.GetRow(i) / norm_f, i);
}
// Calculate sigmas + variance for each state
Matrix* subSigma = new Matrix[m_numStates];
Matrix* subSigmaVar = new Matrix[m_numStates];
for (int s = 0; s < m_numStates; s++)
{
Matrix isubSigmaIn;
Matrix subSigmaOut;
m_sigma[s].GetMatrixSpace(inComponents, inComponents, subSigmaOut);
subSigmaOut.Inverse(isubSigmaIn);
m_sigma[s].GetMatrixSpace(outComponents, inComponents, subSigmaOut);
subSigma[s] = subSigmaOut*isubSigmaIn;
m_sigma[s].GetMatrixSpace(outComponents, outComponents, subSigmaOut);
m_sigma[s].GetMatrixSpace(inComponents, outComponents, isubSigmaIn);
subSigmaVar[s] = subSigmaOut - subSigma[s] * isubSigmaIn;
}
// Now calculate the final mus and sigmas for each data point
Matrix subMuIn;
Matrix subMuOut;
m_mu->GetColumnSpace(outComponents, subMuOut);
m_mu->GetColumnSpace(inComponents, subMuIn);
for (int i = 0; i < outDataSize; i++)
{
// Calculate mu and sigma for the current data point
Matrix sigmaOut(outDim, outDim);
Vector muOut(outDim, true);
for (int s = 0; s < m_numStates; s++)
{
const float pIS = Pxi(i, s);
muOut += (subMuOut.GetRow(s) + (subSigma[s] * (inData.GetRow(i) - subMuIn.GetRow(s)))) * pIS;
sigmaOut += subSigmaVar[s] * (pIS*pIS);
}
// Copy them to the output vars
Fubi::Matrix3f& inverseCov = inverseCovs[i];
Fubi::Vec3f& mean = means[i];
for (unsigned int j = 0; j < outDim; j++)
{
mean[j] = muOut[j];
for (unsigned int k = 0; k < outDim; k++)
inverseCov.c[j][k] = sigmaOut(j, k);
}
// Don't forget to invert the covariance matrix!
inverseCov = inverseCov.inverted();
}
delete[] subSigma;
delete[] subSigmaVar;
}
void Camera::OnUpdate(float dt)
{
Matrix matrix = mParentOffset * mOwner->mMatrix;
//calculate view matrix
mViewMatrix = MatrixLookTo(matrix.r[3], matrix.r[2], matrix.r[1]);
mViewMatrixInv = MatrixInverse(mViewMatrix);
//calculat projection matrix
if (projMode == ProjectionMode::Perspective)
mProjMatrix = MatrixPerspective(perspective.aspectRatio, perspective.FoV,
perspective.farDist, perspective.nearDist);
else
mProjMatrix = MatrixOrtho(ortho.left, ortho.right, ortho.bottom, ortho.top,
ortho.nearDist, ortho.farDist);
mProjMatrixInv = MatrixInverse(mProjMatrix);
//calculate secondary view matrix for motion blur
Matrix rotMatrix = MatrixRotationNormal(mOwner->mAngularVelocity, 0.01f);
Matrix secondaryCameraMatrix;
secondaryCameraMatrix.r[0] = VectorTransform3(matrix.r[0], rotMatrix);
secondaryCameraMatrix.r[1] = VectorTransform3(matrix.r[1], rotMatrix);
secondaryCameraMatrix.r[2] = VectorTransform3(matrix.r[2], rotMatrix);
secondaryCameraMatrix.r[3] = (Vector)matrix.r[3] + mOwner->mVelocity * 0.01f;
Matrix secondaryViewMatrix = MatrixLookTo(secondaryCameraMatrix.r[3], secondaryCameraMatrix.r[2],
secondaryCameraMatrix.r[1]);
mSecondaryProjViewMatrix = secondaryViewMatrix * mProjMatrix;
/*
//calculate secondary view matrix for motion blur
Matrix prevViewMatrix = MatrixLookTo(mPrevMatrix.r[3], mPrevMatrix.r[2], mPrevMatrix.r[1]);
mSecondaryProjViewMatrix = prevViewMatrix * mProjMatrix;
*/
Vector xAxis, yAxis, zAxis;
Vector pos = matrix.GetRow(3);
if (projMode == ProjectionMode::Perspective)
{
float y = tanf(perspective.FoV / 2.0f);
float x = perspective.aspectRatio * y;
mScreenScale = Vector(x, y, 0, 0);
xAxis = x * matrix.GetRow(0);
yAxis = y * matrix.GetRow(1);
zAxis = matrix.GetRow(2);
mFrustum.verticies[0] = pos + perspective.nearDist * (zAxis - xAxis - yAxis);
mFrustum.verticies[1] = pos + perspective.nearDist * (zAxis + xAxis - yAxis);
mFrustum.verticies[2] = pos + perspective.nearDist * (zAxis - xAxis + yAxis);
mFrustum.verticies[3] = pos + perspective.nearDist * (zAxis + xAxis + yAxis);
mFrustum.verticies[4] = pos + perspective.farDist * (zAxis - xAxis - yAxis);
mFrustum.verticies[5] = pos + perspective.farDist * (zAxis + xAxis - yAxis);
mFrustum.verticies[6] = pos + perspective.farDist * (zAxis - xAxis + yAxis);
mFrustum.verticies[7] = pos + perspective.farDist * (zAxis + xAxis + yAxis);
}
else
{
xAxis = matrix.GetRow(0);
yAxis = matrix.GetRow(1);
zAxis = matrix.GetRow(2);
mScreenScale = Vector(1, 1, 0, 0);
mFrustum.verticies[0] = pos + ortho.nearDist * zAxis + ortho.left * xAxis + ortho.bottom *
yAxis;
mFrustum.verticies[1] = pos + ortho.nearDist * zAxis + ortho.right * xAxis + ortho.bottom *
yAxis;
mFrustum.verticies[2] = pos + ortho.nearDist * zAxis + ortho.left * xAxis + ortho.top *
yAxis;
mFrustum.verticies[3] = pos + ortho.nearDist * zAxis + ortho.right * xAxis + ortho.top *
yAxis;
mFrustum.verticies[4] = pos + ortho.farDist * zAxis + ortho.left * xAxis + ortho.bottom *
yAxis;
mFrustum.verticies[5] = pos + ortho.farDist * zAxis + ortho.right * xAxis + ortho.bottom *
yAxis;
mFrustum.verticies[6] = pos + ortho.farDist * zAxis + ortho.left * xAxis + ortho.top *
yAxis;
mFrustum.verticies[7] = pos + ortho.farDist * zAxis + ortho.right * xAxis + ortho.top *
yAxis;
}
mFrustum.CalculatePlanes();
}