//! perform median filtering on a matrix
//! @param[in/out] &matrix reference to the matrix to be filtered
//! @param[in] size size of the median filter
void medianFilter(mat &matrix, int size)
{
unsigned int step = size / 2;
mat tempMat = matrix;
for (unsigned int r = 0; r < matrix.n_rows; r++)
{
for (unsigned int i = 0; i < matrix.n_cols; i++)
{
mat window = zeros<mat>(1, size);
if (i >= step && i <= (matrix.n_cols - 1 - step))
window = matrix.submat(r, i - step, r, i + step);
else
{
if (i < step)
window.submat(0,step,0,size-1) = matrix.submat(r,0,r,step);
else
if (i > (matrix.n_cols - 1 - step))
window.submat(0, 0, 0, step) =
matrix.submat(r, i - step, r, matrix.n_cols - 1);
}
rowvec medianRow = window.row(0);
tempMat(r, i) = median(medianRow);
}
}
matrix = tempMat;
}
//Put the kinect imageframe data onto a Mat
Mat* KOCVStream::kFrameToMat(NUI_IMAGE_FRAME* imageFrame){
Mat* frame = new Mat(height, width, CV_8U);
NUI_LOCKED_RECT LockedRect;
//Lock the imageFrame such that kinnect cannot write on it
INuiFrameTexture* texture = imageFrame->pFrameTexture;
texture->LockRect(0, &LockedRect, NULL, 0);
//Get the kinect depth data
BYTE* imageData;
kinect->getDepthData(&LockedRect);
imageData = kinect->dataPix;
//If the data is not empty convert it to Mat
if (LockedRect.Pitch != 0){
/* //Do not do new above!
frame=new Mat(height, width, CV_8U, imageData);
*/
Mat tempMat(height, width, CV_8U, imageData);
tempMat.copyTo(*frame);
}
else{
return new Mat();
}
//Release the frame
texture->UnlockRect(0);
return frame;
};
void OrientConstRotation::GetValue(TimeValue t, void *val, Interval &valid, GetSetMethod method)
{
if (firstTimeFlag) {
Point3 trans, scaleP;
Quat quat;
Matrix3 tempMat(1);
// CAL-9/26/2002: in absolute mode the value could be un-initialized (random value).
if (method == CTRL_RELATIVE) tempMat = *(Matrix3*)val;
DecomposeMatrix(tempMat, trans, quat, scaleP);
baseRotQuatWorld = baseRotQuatLocal * quat;
baseRotQuatWorld.Normalize();
firstTimeFlag = 0;
}
if (!ivalid.InInterval(t)) {
DbgAssert(val != NULL);
Update(t);
}
valid &= ivalid;
if (method==CTRL_RELATIVE) {
Interval iv;
if (IsLocal()){ // From Update, I'm getting target_local_TM in curRot
Matrix3 *mat = (Matrix3*)val; // this is source_Parent_TM
Quat q = curRot; // this is target_local_TM
PreRotateMatrix(*mat, q); // source_world_TM = source_Parent_TM * target_local_TM
}
else { // i.e., WorldToWorld: from Update, I'm getting target_world_TM in curRot
int ct = pblock->Count(orientation_target_list);;
if (ct < 1){
Matrix3 *mat = (Matrix3*)val;
Quat q = curRot;
PreRotateMatrix(*mat, q);
}
else{
Matrix3 *mat = (Matrix3*)val; // this is source_Parent_TM
// CAL-06/24/02: preserve all components and replace with the new orientation.
AffineParts ap;
decomp_affine( *mat, &ap );
ap.q = curRot; // target world rotation
comp_affine( ap, *mat );
// CAL-06/24/02: this only preserve the translation component
// Point3 tr;
// tr = mat->GetTrans();
// mat->IdentityMatrix();
// Quat q = curRot; // this is target_world_TM
// q.MakeMatrix(*mat);
// mat->SetTrans(tr);
}
}
}
else {
*((Quat*)val) = curRot;
}
// RedrawListbox(GetCOREInterface()->GetTime());
}
void StrColRoSeService::encodeHeightmap(std::vector<unsigned char>& encodedHeightmap, const cv::Mat& heightmap,
float widthRatio, float heightRatio){
unsigned int smallWidth = (float) heightmap.cols * (float) widthRatio;
unsigned int smallHeight = (float) heightmap.rows * (float) heightRatio;
cv::Mat tempMat(heightmap, cv::Rect(0, 0, smallWidth, smallHeight));
cv::Mat img;
tempMat.convertTo(img, CV_8U, 255);
std::vector<int> params(2);
params[0] = CV_IMWRITE_PNG_COMPRESSION;
params[1] = 9;
cv::imencode(".png", img, encodedHeightmap, params);
}
//=============================================================================
int Epetra_SerialDenseSVD::Factor() {
int ierr = 0;
ANORM_ = Matrix_->OneNorm(); // Compute 1-Norm of A
//allocate U_, S_, and Vt_ if not already done
if(U_==0)
{
U_ = new double[M_*N_];
S_ = new double[M_];
Vt_ = new double[M_*N_];
}
else //zero them out
{
for( int i = 0; i < M_; ++i ) S_[i]=0.0;
for( int i = 0; i < M_*N_; ++i )
{
U_[i]=0.0;
Vt_[i]=0.0;
}
}
// if (Equilibrate_) ierr = EquilibrateMatrix();
if (ierr!=0) EPETRA_CHK_ERR(ierr-2);
//allocate temp work space
int lwork = 5*M_;
double *work = new double[lwork];
char job = 'A';
//create temporary matrix to avoid writeover of original
Epetra_SerialDenseMatrix tempMat( *Matrix_ );
GESVD( job, job, M_, N_, tempMat.A(), LDA_, S_, U_, N_, Vt_, M_, work, &lwork, &INFO_ );
delete [] work;
Factored_ = true;
double DN = N_;
UpdateFlops(2.0*(DN*DN*DN)/3.0);
EPETRA_CHK_ERR(INFO_);
return(0);
}
int Pretreatment::RemoveReflective(const Mat &morphologyMat, Mat &opaqueMat)
{
vector< vector<Point> > contours;
findContours(morphologyMat, contours, CV_RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);
double *SumOfArea = new double[contours.size()]; // Contours' area.
Moments *moment = new Moments[contours.size()]; // Contours' moments.
Point *central = new Point[contours.size()]; // Contours' central point.
for(unsigned int i = 0; i < contours.size(); i++)
{
SumOfArea[i] = contourArea(contours[i], false);
moment[i] = moments(contours[i], false);
central[i] = Point(static_cast<int>(moment[i].m10 / moment[i].m00), static_cast<int>( moment[i].m01 / moment[i].m00));
}
double stv = Utility::Instance()->StandardDeviation(SumOfArea, contours.size());
int Neighborhood;
double AreaDiff, centralDiff;
double DistancePenalty;
Mat tempMat(morphologyMat.rows, morphologyMat.cols, CV_8UC1, Scalar(255));
for(unsigned int i = 0; i < contours.size(); i++)
{
Neighborhood = Utility::Instance()->NearestNeighborhoodPoint(central, contours.size(), central[i]);
printf("Neighborhood %d\r\n", Neighborhood);
AreaDiff = SumOfArea[i] - SumOfArea[Neighborhood];
centralDiff = Utility::Instance()->EuclideanDistance(central[i], central[Neighborhood]);
if( (abs(AreaDiff)-stv) < (SumOfArea[i] * ((abs(central[i].y-central[Neighborhood].y)/centralDiff)+1)))
{
if(centralDiff/DistancePenalty < 2.5 || i == 0)
{
drawContours(tempMat, contours, i, Scalar(0), 1 );
DistancePenalty = centralDiff;
}
}
}
floodFill(tempMat, Point(0, 0), Scalar(0));
tempMat.copyTo(opaqueMat);
return 0;
}
void ProcessedShaderMaterial::setSceneInfo(SceneRenderState * state, const SceneData& sgData, U32 pass)
{
PROFILE_SCOPE( ProcessedShaderMaterial_setSceneInfo );
GFXShaderConstBuffer* shaderConsts = _getShaderConstBuffer(pass);
ShaderConstHandles* handles = _getShaderConstHandles(pass);
// Set cubemap stuff here (it's convenient!)
const Point3F &eyePosWorld = state->getCameraPosition();
if ( handles->mCubeEyePosSC->isValid() )
{
if(_hasCubemap(pass) || mMaterial->mDynamicCubemap)
{
Point3F cubeEyePos = eyePosWorld - sgData.objTrans->getPosition();
shaderConsts->set(handles->mCubeEyePosSC, cubeEyePos);
}
}
shaderConsts->setSafe(handles->mVisiblitySC, sgData.visibility);
shaderConsts->setSafe(handles->mEyePosWorldSC, eyePosWorld);
if ( handles->mEyePosSC->isValid() )
{
MatrixF tempMat( *sgData.objTrans );
tempMat.inverse();
Point3F eyepos;
tempMat.mulP( eyePosWorld, &eyepos );
shaderConsts->set(handles->mEyePosSC, eyepos);
}
shaderConsts->setSafe(handles->mEyeMatSC, state->getCameraTransform());
ShaderRenderPassData *rpd = _getRPD( pass );
for ( U32 i=0; i < rpd->featureShaderHandles.size(); i++ )
rpd->featureShaderHandles[i]->setConsts( state, sgData, shaderConsts );
LIGHTMGR->setLightInfo( this, mMaterial, sgData, state, pass, shaderConsts );
}
int Epetra_FEVbrMatrix::GlobalAssemble(bool callFillComplete)
{
if(Map().Comm().NumProc() < 2 || ignoreNonLocalEntries_) {
if(callFillComplete) {
EPETRA_CHK_ERR(FillComplete());
}
return(0);
}
int i;
//In this method we need to gather all the non-local (overlapping) data
//that's been input on each processor, into the
//non-overlapping distribution defined by the map that 'this' matrix was
//constructed with.
//Need to build a map that describes our nonlocal data.
//First, create a list of the sizes (point-rows per block-row) of the
//nonlocal rows we're holding.
int* pointRowsPerNonlocalBlockRow = numNonlocalBlockRows_>0 ?
new int[numNonlocalBlockRows_] : NULL;
for(i=0; i<numNonlocalBlockRows_; ++i) {
pointRowsPerNonlocalBlockRow[i] = nonlocalCoefs_[i][0]->M();
}
//We'll use the arbitrary distribution constructor of BlockMap.
Epetra_BlockMap sourceMap(-1, numNonlocalBlockRows_, nonlocalBlockRows_, // CJ TODO FIXME long long
pointRowsPerNonlocalBlockRow,
RowMap().IndexBase(), RowMap().Comm());
delete [] pointRowsPerNonlocalBlockRow;
//If sourceMap has global size 0, then no nonlocal data exists and we can
//skip most of this function.
if(sourceMap.NumGlobalElements64() < 1) {
if(callFillComplete) {
EPETRA_CHK_ERR(FillComplete());
}
return(0);
}
//We also need to build a column-map, containing the columns in our
//nonlocal data. To do that, create a list of all column-indices that
//occur in our nonlocal rows.
int numCols = 0, allocLen = 0;
int* cols = NULL;
int* pointColsPerBlockCol = NULL;
int ptColAllocLen = 0;
int insertPoint = -1;
for(i=0; i<numNonlocalBlockRows_; ++i) {
for(int j=0; j<nonlocalBlockRowLengths_[i]; ++j) {
int col = nonlocalBlockCols_[i][j];
int offset = Epetra_Util_binary_search(col, cols, numCols, insertPoint);
if (offset < 0) {
EPETRA_CHK_ERR( Epetra_Util_insert(col, insertPoint, cols,
numCols, allocLen) );
int tmpNumCols = numCols-1;
EPETRA_CHK_ERR( Epetra_Util_insert(nonlocalCoefs_[i][j]->N(),
insertPoint,
pointColsPerBlockCol,
tmpNumCols, ptColAllocLen) );
}
}
}
Epetra_BlockMap colMap(-1, numCols, cols, // CJ TODO FIXME long long
pointColsPerBlockCol,
RowMap().IndexBase(), RowMap().Comm());
delete [] cols;
delete [] pointColsPerBlockCol;
numCols = 0;
allocLen = 0;
//now we need to create a matrix with sourceMap and colMap, and fill it with
//our nonlocal data so we can then export it to the correct owning
//processors.
Epetra_VbrMatrix tempMat(Copy, sourceMap, colMap, nonlocalBlockRowLengths_);
//Next we need to make sure the 'indices-are-global' attribute of tempMat's
//graph is set to true, in case this processor doesn't end up calling the
//InsertGlobalValues method...
const Epetra_CrsGraph& graph = tempMat.Graph();
Epetra_CrsGraph& nonconst_graph = const_cast<Epetra_CrsGraph&>(graph);
nonconst_graph.SetIndicesAreGlobal(true);
for(i=0; i<numNonlocalBlockRows_; ++i) {
EPETRA_CHK_ERR( tempMat.BeginInsertGlobalValues(nonlocalBlockRows_[i],
nonlocalBlockRowLengths_[i],
nonlocalBlockCols_[i]) );
for(int j=0; j<nonlocalBlockRowLengths_[i]; ++j) {
Epetra_SerialDenseMatrix& subblock = *(nonlocalCoefs_[i][j]);
EPETRA_CHK_ERR( tempMat.SubmitBlockEntry(subblock.A(),
subblock.LDA(),
subblock.M(),
subblock.N()) );
}
EPETRA_CHK_ERR( tempMat.EndSubmitEntries() );
}
//Now we need to call FillComplete on our temp matrix. We need to
//pass a DomainMap and RangeMap, which are not the same as the RowMap
//and ColMap that we constructed the matrix with.
EPETRA_CHK_ERR(tempMat.FillComplete(RowMap(), sourceMap));
//Finally, we're ready to create the exporter and export non-local data to
//the appropriate owning processors.
Epetra_Export exporter(sourceMap, RowMap());
EPETRA_CHK_ERR( Export(tempMat, exporter, Add) );
if(callFillComplete) {
EPETRA_CHK_ERR(FillComplete());
}
destroyNonlocalData();
return(0);
}
void BachelorThesis::loadImage()
{
if( videoReader.isOpen() && !isVideoPaused )
{
timer.start();
cv::gpu::GpuMat processedImage;
cv::Mat unProcessedImage;
// loading new frames. the amount of skipped frames is indicated by playbackSpeed
for( int i = 0; i < playbackSpeed; i++ )
{
if( videoReader.selectedType == VideoReader::Type::CPU || videoReader.selectedType == VideoReader::Type::LIVE )
{
cv::Mat temp = videoReader.getNextImage();
cv::cvtColor( temp, temp, CV_BGR2RGBA );
unProcessedImage = temp;
if( originalImage.cols != temp.cols || originalImage.rows != temp.rows )
{
originalImage = cv::gpu::GpuMat( 640, 480, CV_8UC4 );
}
originalImage.upload( temp );
} else if ( videoReader.selectedType == VideoReader::Type::GPU )
{
originalImage = videoReader.getNextImage_GPU();
}
}
// get the selected area
QPoint maxSize( originalImage.cols, originalImage.rows );
QRect roi = roiSelector->geometry();
// adjust the roisize according to the maximum dimensions
adjustRoiSize( roiSelector->geometry(), roi, maxSize );
// initializes a new section gpumat with the size of the roi and the imagetype of the incoming image
cv::gpu::GpuMat section( roi.width(), roi.height(), originalImage.type() );
// init a cv::Rect with the properties of the ROI
cv::Rect cvSelectedRect = cv::Rect( roi.x(), roi.y(), roi.width(), roi.height() );
// a new local copy of the current image
cv::gpu::GpuMat tempMat(originalImage );
// select a part of this new image ( position and size is stored in the passed cv::Rect) and copy this to the new image
tempMat(cvSelectedRect).copyTo( section );
// add the cropped image to the processing pipeline
pipeline->addImage( §ion );
// start the pipeline ( do the processing )
pipeline->start();
// get the processed image
processedImage = pipeline->getFinishedImage();
// make a local copy of the entire unprocessed original image
cv::gpu::GpuMat finalImage( originalImage );
// copy the processed image into the original image, exactly at the location of the ROI
processedImage.copyTo( finalImage( cvSelectedRect ) );
// convert the cv::gpu::GpuMat into a QPixmap
QPixmap imagePixmap = QPixmap::fromImage( this->mat2QImage( cv::Mat( finalImage ) ) );
// display the QPixmap onto the label
ui.videoLabel->setPixmap( imagePixmap );
ui.videoLabel->setMaximumHeight( imagePixmap.width() );
ui.videoLabel->setMaximumWidth( imagePixmap.height() );
ui.videoLabel->adjustSize();
QPixmap originalImagePixmap = QPixmap::fromImage( this->mat2QImage( cv::Mat( unProcessedImage ) ) );
ui.originalVideoLabel->setPixmap( originalImagePixmap );
ui.originalVideoLabel->setMaximumHeight( originalImagePixmap.width() );
ui.originalVideoLabel->setMaximumWidth( originalImagePixmap.height() );
ui.originalVideoLabel->adjustSize();
timer.stop();
timer.store();
//std::cout << "it took by average:" << timer.getAverageTimeStdString() << "ms." << std::endl;
//std::cout << "lates was: " << timer.getLatestStdString() << "ms." << std::endl;
QString elapsed;
elapsed.append( QString( "%1" ).arg( videoReader.getNormalizedProgress() ) );
ui.progressBar->setValue( videoReader.getCurrentFrameNr() );
}
else
{
// no new frame. do nothings
}
}
// This is the model equation for the timeframe RANSAC
// B.K.P. Horn's closed form Absolute Orientation method (1987 paper)
// The convention used here is: right = (1/scale) * RMat * left + TMat
int Photogrammetry::absoluteOrientation(vector<cv::Point3d> & left, vector<cv::Point3d> & right, cv::Mat & RMat, cv::Mat & TMat, double & scale) {
//check if both vectors have the same number of size
if (left.size() != right.size()) {
cerr << "Sizes don't match" << endl;
return -1;
}
//compute the mean of the left and right set of points
cv::Point3d leftmean, rightmean;
leftmean.x = 0;
leftmean.y = 0;
leftmean.z = 0;
rightmean.x = 0;
rightmean.y = 0;
rightmean.z = 0;
for (int i = 0; i < left.size(); i++) {
leftmean.x += left[i].x;
leftmean.y += left[i].y;
leftmean.z += left[i].z;
rightmean.x += right[i].x;
rightmean.y += right[i].y;
rightmean.z += right[i].z;
}
leftmean.x /= left.size();
leftmean.y /= left.size();
leftmean.z /= left.size();
rightmean.x /= right.size();
rightmean.y /= right.size();
rightmean.z /= right.size();
cv::Mat leftmeanMat(3,1,CV_64F);
cv::Mat rightmeanMat(3,1,CV_64F);
leftmeanMat.at<double>(0,0) = leftmean.x;
leftmeanMat.at<double>(0,1) = leftmean.y;
leftmeanMat.at<double>(0,2) = leftmean.z;
rightmeanMat.at<double>(0,0) = rightmean.x;
rightmeanMat.at<double>(0,1) = rightmean.y;
rightmeanMat.at<double>(0,2) = rightmean.z;
//normalize all points
for (int i = 0; i < left.size(); i++) {
left[i].x -= leftmean.x;
left[i].y -= leftmean.y;
left[i].z -= leftmean.z;
right[i].x -= rightmean.x;
right[i].y -= rightmean.y;
right[i].z -= rightmean.z;
}
//compute scale (use the symmetrical solution)
double Sl = 0;
double Sr = 0;
// this is the symmetrical version of the scale !
for (int i = 0; i < left.size(); i++) {
Sl += left[i].x*left[i].x + left[i].y*left[i].y + left[i].z*left[i].z;
Sr += right[i].x*right[i].x + right[i].y*right[i].y + right[i].z*right[i].z;
}
scale = sqrt(Sr/Sl);
// cout << "Scale: " << scale << endl;
//create M matrix
double M[3][3];// = {0.0};
/*
// I believe this is wrong, since not summing over all left right elements, just for the last element ! KM Nov 21
for (int i = 0; i < left.size(); i++) {
M[0][0] = left[i].x*right[i].x;
M[0][1] = left[i].x*right[i].y;
M[0][2] = left[i].x*right[i].z;
M[1][0] = left[i].y*right[i].x;
M[1][1] = left[i].y*right[i].y;
M[1][2] = left[i].y*right[i].z;
M[2][0] = left[i].z*right[i].x;
M[2][1] = left[i].z*right[i].y;
M[2][2] = left[i].z*right[i].z;
}
*/
M[0][0] = 0;
M[0][1] = 0;
M[0][2] = 0;
M[1][0] = 0;
M[1][1] = 0;
M[1][2] = 0;
M[2][0] = 0;
M[2][1] = 0;
M[2][2] = 0;
for (int i = 0; i < left.size(); i++)
{
M[0][0] += left[i].x*right[i].x;
M[0][1] += left[i].x*right[i].y;
M[0][2] += left[i].x*right[i].z;
M[1][0] += left[i].y*right[i].x;
M[1][1] += left[i].y*right[i].y;
M[1][2] += left[i].y*right[i].z;
M[2][0] += left[i].z*right[i].x;
M[2][1] += left[i].z*right[i].y;
M[2][2] += left[i].z*right[i].z;
}
//create N matrix
cv::Mat N = cv::Mat::zeros(4,4,CV_64F);
N.at<double>(0,0) = M[0][0] + M[1][1] + M[2][2];
N.at<double>(0,1) = M[1][2] - M[2][1];
N.at<double>(0,2) = M[2][0] - M[0][2];
N.at<double>(0,3) = M[0][1] - M[1][0];
N.at<double>(1,0) = M[1][2] - M[2][1];
N.at<double>(1,1) = M[0][0] - M[1][1] - M[2][2];
N.at<double>(1,2) = M[0][1] + M[1][0];
N.at<double>(1,3) = M[2][0] + M[0][2];
N.at<double>(2,0) = M[2][0] - M[0][2];
N.at<double>(2,1) = M[0][1] + M[1][0];
N.at<double>(2,2) = -M[0][0] + M[1][1] - M[2][2];
N.at<double>(2,3) = M[1][2] + M[2][1];
N.at<double>(3,0) = M[0][1] - M[1][0];
N.at<double>(3,1) = M[2][0] + M[0][2];
N.at<double>(3,2) = M[1][2] + M[2][1];
N.at<double>(3,3) = -M[0][0] - M[1][1] + M[2][2];
// cout << "N: " << N << endl;
//compute eigenvalues
cv::Mat eigenvalues(1,4,CV_64FC1);
cv::Mat eigenvectors(4,4,CV_64FC1);
// cout << "eigenvalues: \n" << eigenvalues << endl;
if (!cv::eigen(N, eigenvalues, eigenvectors)) {
cerr << "eigen failed" << endl;
return -1;
}
// cout << "Eigenvalues:\n" << eigenvalues << endl;
// cout << "Eigenvectors:\n" << eigenvectors << endl;
//compute quaterion as maximum eigenvector
double q[4];
q[0] = eigenvectors.at<double>(0,0);
q[1] = eigenvectors.at<double>(0,1);
q[2] = eigenvectors.at<double>(0,2);
q[3] = eigenvectors.at<double>(0,3);
/* // I believe this changed with the openCV implementation, eigenvectors are stored in row-order !
q[0] = eigenvectors.at<double>(0,0);
q[1] = eigenvectors.at<double>(1,0);
q[2] = eigenvectors.at<double>(2,0);
q[3] = eigenvectors.at<double>(3,0);
*/
double absOfEigVec = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
q[0] /= absOfEigVec;
q[1] /= absOfEigVec;
q[2] /= absOfEigVec;
q[3] /= absOfEigVec;
cv::Mat qMat(4,1,CV_64F,q);
// cout << "q: " << qMat << endl;
//compute Rotation matrix
RMat.at<double>(0,0) = q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3];
RMat.at<double>(0,1) = 2*(q[1]*q[2] - q[0]*q[3]);
RMat.at<double>(0,2) = 2*(q[1]*q[3] + q[0]*q[2]);
RMat.at<double>(1,0) = 2*(q[2]*q[1] + q[0]*q[3]);
RMat.at<double>(1,1) = q[0]*q[0] - q[1]*q[1] + q[2]*q[2] - q[3]*q[3];
RMat.at<double>(1,2) = 2*(q[2]*q[3] - q[0]*q[1]);
RMat.at<double>(2,0) = 2*(q[3]*q[1] - q[0]*q[2]);
RMat.at<double>(2,1) = 2*(q[2]*q[3] + q[0]*q[1]);
RMat.at<double>(2,2) = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
// cout <<"R:\n" << RMat << endl;
// cout << "Det: " << determinant(RMat) << endl;
//find translation
cv::Mat tempMat(3,1,CV_64F);
//gemm(RMat, leftmeanMat, -1.0, rightmeanMat, 1.0, TMat); // enforcing scale of 1, since same scales in both frames
// The convention used here is: right = (1/scale) * RMat * left + TMat
TMat = -(1/scale) * RMat*leftmeanMat + rightmeanMat;
// gemm(RMat, leftmeanMat, -1.0 * scale, rightmeanMat, 1.0, TMat);
// cout << "Translation: " << TMat << endl;
return 0;
}
void UnwrapMod::AlignCluster(int baseCluster, int moveCluster, int innerFaceIndex, int outerFaceIndex,int edgeIndex)
{
//get edges that are coincedent
int vInner[2];
int vOuter[2];
int vInnerVec[2];
int vOuterVec[2];
int ct = 0;
int vct = 0;
for (int i = 0; i < TVMaps.f[innerFaceIndex]->count; i++)
{
int innerIndex = TVMaps.f[innerFaceIndex]->v[i];
for (int j = 0; j < TVMaps.f[outerFaceIndex]->count; j++)
{
int outerIndex = TVMaps.f[outerFaceIndex]->v[j];
if (innerIndex == outerIndex)
{
vInner[ct] = TVMaps.f[innerFaceIndex]->t[i];
vOuter[ct] = TVMaps.f[outerFaceIndex]->t[j];
ct++;
}
}
}
vInnerVec[0] = -1;
vInnerVec[1] = -1;
vOuterVec[0] = -1;
vOuterVec[1] = -1;
ct = 0;
if ( (TVMaps.f[innerFaceIndex]->flags & FLAG_CURVEDMAPPING) && (TVMaps.f[innerFaceIndex]->vecs) &&
(TVMaps.f[outerFaceIndex]->flags & FLAG_CURVEDMAPPING) && (TVMaps.f[outerFaceIndex]->vecs)
)
{
for (i = 0; i < TVMaps.f[innerFaceIndex]->count*2; i++)
{
int innerIndex = TVMaps.f[innerFaceIndex]->vecs->vhandles[i];
for (int j = 0; j < TVMaps.f[outerFaceIndex]->count*2; j++)
{
int outerIndex = TVMaps.f[outerFaceIndex]->vecs->vhandles[j];
if (innerIndex == outerIndex)
{
int vec = TVMaps.f[innerFaceIndex]->vecs->handles[i];
vInnerVec[ct] = vec;
vec = TVMaps.f[outerFaceIndex]->vecs->handles[j];
vOuterVec[ct] = vec;
ct++;
}
}
}
}
//get align vector
Point3 pInner[2];
Point3 pOuter[2];
pInner[0] = TVMaps.v[vInner[0]].p;
pInner[1] = TVMaps.v[vInner[1]].p;
pOuter[0] = TVMaps.v[vOuter[0]].p;
pOuter[1] = TVMaps.v[vOuter[1]].p;
Point3 offset = pInner[0] - pOuter[0];
Point3 vecA, vecB;
vecA = Normalize(pInner[1] - pInner[0]);
vecB = Normalize(pOuter[1] - pOuter[0]);
float dot = DotProd(vecA,vecB);
float angle = 0.0f;
if (dot == -1.0f)
angle = PI;
else if (dot == 1.0f)
angle = 0.f;
else angle = acos(dot);
if ((_isnan(angle)) || (!_finite(angle)))
angle = 0.0f;
// DebugPrint("Stop\n");
// angle = acos(dot);
//DebugPrint("angle %f dot %f \n",angle, dot);
/*
DebugPrint(" VecA %f %f %f \n",vecA.x,vecA.y,vecA.z);
DebugPrint(" VecB %f %f %f \n",vecB.x,vecB.y,vecB.z);
*/
Matrix3 tempMat(1);
tempMat.RotateZ(angle);
Point3 vecC = VectorTransform(tempMat,vecB);
float negAngle = -angle;
Matrix3 tempMat2(1);
tempMat2.RotateZ(negAngle);
Point3 vecD = VectorTransform(tempMat2,vecB);
float la,lb;
la = Length(vecA-vecC);
lb = Length(vecA-vecD);
if (la > lb)
angle = negAngle;
clusterList[moveCluster]->newX = offset.x;
clusterList[moveCluster]->newY = offset.y;
//build vert list
//move those verts
BitArray processVertList;
processVertList.SetSize(TVMaps.v.Count());
processVertList.ClearAll();
for (i =0; i < clusterList[moveCluster]->faces.Count(); i++)
{
int faceIndex = clusterList[moveCluster]->faces[i];
for (int j =0; j < TVMaps.f[faceIndex]->count; j++)
{
int vertexIndex = TVMaps.f[faceIndex]->t[j];
processVertList.Set(vertexIndex);
if ( (objType == IS_PATCH) && (TVMaps.f[faceIndex]->flags & FLAG_CURVEDMAPPING) && (TVMaps.f[faceIndex]->vecs))
{
int vertIndex;
if (TVMaps.f[faceIndex]->flags & FLAG_INTERIOR)
{
vertIndex = TVMaps.f[faceIndex]->vecs->interiors[j];
if ((vertIndex >=0) && (vertIndex < processVertList.GetSize()))
processVertList.Set(vertIndex);
}
vertIndex = TVMaps.f[faceIndex]->vecs->handles[j*2];
if ((vertIndex >=0) && (vertIndex < processVertList.GetSize()))
processVertList.Set(vertIndex);
vertIndex = TVMaps.f[faceIndex]->vecs->handles[j*2+1];
if ((vertIndex >=0) && (vertIndex < processVertList.GetSize()))
processVertList.Set(vertIndex);
}
}
}
for (i = 0; i < processVertList.GetSize(); i++)
{
if (processVertList[i])
{
//DebugPrint("%d ",i);
Point3 p = TVMaps.v[i].p;
//move to origin
p -= pOuter[0];
//rotate
Matrix3 mat(1);
mat.RotateZ(angle);
p = p * mat;
//move to anchor point
p += pInner[0];
TVMaps.v[i].p = p;
if (TVMaps.cont[i]) TVMaps.cont[i]->SetValue(0,&TVMaps.v[i].p);
}
}
if ((vInnerVec[0] != -1) && (vInnerVec[1] != -1) && (vOuterVec[0] != -1) && (vOuterVec[1] != -1))
{
TVMaps.v[vOuterVec[0]].p = TVMaps.v[vInnerVec[0]].p;
if (TVMaps.cont[vOuterVec[0]]) TVMaps.cont[vOuterVec[0]]->SetValue(0,&TVMaps.v[vInnerVec[0]].p);
TVMaps.v[vOuterVec[1]].p = TVMaps.v[vInnerVec[1]].p;
if (TVMaps.cont[vOuterVec[1]]) TVMaps.cont[vOuterVec[1]]->SetValue(0,&TVMaps.v[vInnerVec[1]].p);
}
}
void HoverVehicle::updateForces(F32 /*dt*/)
{
PROFILE_SCOPE( HoverVehicle_UpdateForces );
Point3F gravForce(0, 0, sHoverVehicleGravity * mRigid.mass * mGravityMod);
MatrixF currTransform;
mRigid.getTransform(&currTransform);
mRigid.atRest = false;
mThrustLevel = (mForwardThrust * mDataBlock->mainThrustForce +
mReverseThrust * mDataBlock->reverseThrustForce +
mLeftThrust * mDataBlock->strafeThrustForce +
mRightThrust * mDataBlock->strafeThrustForce);
Point3F thrustForce = ((Point3F( 0, 1, 0) * (mForwardThrust * mDataBlock->mainThrustForce)) +
(Point3F( 0, -1, 0) * (mReverseThrust * mDataBlock->reverseThrustForce)) +
(Point3F(-1, 0, 0) * (mLeftThrust * mDataBlock->strafeThrustForce)) +
(Point3F( 1, 0, 0) * (mRightThrust * mDataBlock->strafeThrustForce)));
currTransform.mulV(thrustForce);
if (mJetting)
thrustForce *= mDataBlock->turboFactor;
Point3F torque(0, 0, 0);
Point3F force(0, 0, 0);
Point3F vel = mRigid.linVelocity;
F32 baseStabLen = getBaseStabilizerLength();
Point3F stabExtend(0, 0, -baseStabLen);
currTransform.mulV(stabExtend);
StabPoint stabPoints[2];
stabPoints[0].osPoint = Point3F((mObjBox.minExtents.x + mObjBox.maxExtents.x) * 0.5,
mObjBox.maxExtents.y,
(mObjBox.minExtents.z + mObjBox.maxExtents.z) * 0.5);
stabPoints[1].osPoint = Point3F((mObjBox.minExtents.x + mObjBox.maxExtents.x) * 0.5,
mObjBox.minExtents.y,
(mObjBox.minExtents.z + mObjBox.maxExtents.z) * 0.5);
U32 j, i;
for (i = 0; i < 2; i++) {
currTransform.mulP(stabPoints[i].osPoint, &stabPoints[i].wsPoint);
stabPoints[i].wsExtension = stabExtend;
stabPoints[i].extension = baseStabLen;
stabPoints[i].wsVelocity = mRigid.linVelocity;
}
RayInfo rinfo;
mFloating = true;
bool reallyFloating = true;
F32 compression[2] = { 0.0f, 0.0f };
F32 normalMod[2] = { 0.0f, 0.0f };
bool normalSet[2] = { false, false };
Point3F normal[2];
for (j = 0; j < 2; j++) {
if (getContainer()->castRay(stabPoints[j].wsPoint, stabPoints[j].wsPoint + stabPoints[j].wsExtension * 2.0,
TerrainObjectType |
WaterObjectType, &rinfo))
{
reallyFloating = false;
if (rinfo.t <= 0.5) {
// Ok, stab is in contact with the ground, let's calc the forces...
compression[j] = (1.0 - (rinfo.t * 2.0)) * baseStabLen;
}
normalSet[j] = true;
normalMod[j] = rinfo.t < 0.5 ? 1.0 : (1.0 - ((rinfo.t - 0.5) * 2.0));
normal[j] = rinfo.normal;
}
if ( pointInWater( stabPoints[j].wsPoint ) )
compression[j] = baseStabLen;
}
for (j = 0; j < 2; j++) {
if (compression[j] != 0.0) {
mFloating = false;
// Spring force and damping
Point3F springForce = -stabPoints[j].wsExtension;
springForce.normalize();
springForce *= compression[j] * mDataBlock->stabSpringConstant;
Point3F springDamping = -stabPoints[j].wsExtension;
springDamping.normalize();
springDamping *= -getMin(mDot(springDamping, stabPoints[j].wsVelocity), 0.7f) * mDataBlock->stabDampingConstant;
force += springForce + springDamping;
}
}
// Gravity
if (reallyFloating == false)
force += gravForce;
else
force += gravForce * mDataBlock->floatingGravMag;
// Braking
F32 vellen = mRigid.linVelocity.len();
if (mThrottle == 0.0f &&
mLeftThrust == 0.0f &&
mRightThrust == 0.0f &&
vellen != 0.0f &&
vellen < mDataBlock->brakingActivationSpeed)
{
Point3F dir = mRigid.linVelocity;
dir.normalize();
dir.neg();
force += dir * mDataBlock->brakingForce;
}
// Gyro Drag
torque = -mRigid.angMomentum * mDataBlock->gyroDrag;
// Move to proper normal
Point3F sn, r;
currTransform.getColumn(2, &sn);
if (normalSet[0] || normalSet[1]) {
if (normalSet[0] && normalSet[1]) {
F32 dot = mDot(normal[0], normal[1]);
if (dot > 0.999) {
// Just pick the first normal. They're too close to call
if ((sn - normal[0]).lenSquared() > 0.00001) {
mCross(sn, normal[0], &r);
torque += r * mDataBlock->normalForce * normalMod[0];
}
} else {
Point3F rotAxis;
mCross(normal[0], normal[1], &rotAxis);
rotAxis.normalize();
F32 angle = mAcos(dot) * (normalMod[0] / (normalMod[0] + normalMod[1]));
AngAxisF aa(rotAxis, angle);
QuatF q(aa);
MatrixF tempMat(true);
q.setMatrix(&tempMat);
Point3F newNormal;
tempMat.mulV(normal[1], &newNormal);
if ((sn - newNormal).lenSquared() > 0.00001) {
mCross(sn, newNormal, &r);
torque += r * (mDataBlock->normalForce * ((normalMod[0] + normalMod[1]) * 0.5));
}
}
} else {
Point3F useNormal;
F32 useMod;
if (normalSet[0]) {
useNormal = normal[0];
useMod = normalMod[0];
} else {
useNormal = normal[1];
useMod = normalMod[1];
}
if ((sn - useNormal).lenSquared() > 0.00001) {
mCross(sn, useNormal, &r);
torque += r * mDataBlock->normalForce * useMod;
}
}
} else {
if ((sn - Point3F(0, 0, 1)).lenSquared() > 0.00001) {
mCross(sn, Point3F(0, 0, 1), &r);
torque += r * mDataBlock->restorativeForce;
}
}
Point3F sn2;
currTransform.getColumn(0, &sn);
currTransform.getColumn(1, &sn2);
mCross(sn, sn2, &r);
r.normalize();
torque -= r * (mSteering.x * mDataBlock->steeringForce);
currTransform.getColumn(0, &sn);
currTransform.getColumn(2, &sn2);
mCross(sn, sn2, &r);
r.normalize();
torque -= r * (mSteering.x * mDataBlock->rollForce);
currTransform.getColumn(1, &sn);
currTransform.getColumn(2, &sn2);
mCross(sn, sn2, &r);
r.normalize();
torque -= r * (mSteering.y * mDataBlock->pitchForce);
// Apply drag
Point3F vDrag = mRigid.linVelocity;
if (!mFloating) {
vDrag.convolve(Point3F(1, 1, mDataBlock->vertFactor));
} else {
vDrag.convolve(Point3F(0.25, 0.25, mDataBlock->vertFactor));
}
force -= vDrag * mDataBlock->dragForce;
force += mFloating ? thrustForce * mDataBlock->floatingThrustFactor : thrustForce;
// Add in physical zone force
force += mAppliedForce;
// Container buoyancy & drag
force += Point3F(0, 0,-mBuoyancy * sHoverVehicleGravity * mRigid.mass * mGravityMod);
force -= mRigid.linVelocity * mDrag;
torque -= mRigid.angMomentum * mDrag;
mRigid.force = force;
mRigid.torque = torque;
}
