sunwen/UR
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix geometryInfo bug with 2 motorpositions.

Browse Source
This commit is contained in:
sunwen
2023-06-27 09:10:50 +08:00
parent a5ecf49433
commit aded4a1a77
1 changed files with 23 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

53
src/common/getGeometryInfo.cpp
View File

@@ -3,6 +3,7 @@
#include "Function1D.h"
#include "Function.h"
#include "Function2D.h"
#include "Function3D.h"
#include "Matrix.h"
#include "src/common/getMeasurementMetaData.h"
@@ -63,60 +64,52 @@ void transformGeometry(const Matrix& aMotorPos, const Matrix& aTransformationMat
const Matrix aRlList, const Matrix aRnList, const Matrix aSlList, const Matrix aSnList, GeometryInfo& aOutput)
{
double motorPos = max(aMotorPos)[0];
size_t receiverMatrixSize = motorPos * max(aRlList)[0] * max(aRnList)[0] * 3;
size_t senderMatrixSize = motorPos * max(aSlList)[0] * max(aSnList)[0] * 3;
for(int m=0; m<motorPos; ++m)
{
Matrix transMat = aTransformationMatrices($,$,m).toMatrix();
Matrix transMatNormals = transpose(inv(transMat));
double* receiverNormalsData = new double[receiverMatrixSize];
double* receiverNormalsDataCopyPointer = receiverNormalsData;
double* receiverPositionsData = new double[receiverMatrixSize];
double* receiverPositionsCopyPointer = receiverPositionsData;
Matrix receiverNormal = Aurora::zeros(3, max(aRnList)[0], max(aRlList)[0]) + NAN;
Matrix receiverPosition = Aurora::zeros(3, max(aRnList)[0], max(aRlList)[0]) + NAN;
for(int i=0; i<aRlList.getDataSize(); ++i)
{
for(int j=0; j<aRnList.getDataSize(); ++j)
{
Matrix receiverNormalsMatrix = Recon::receiverNormals[i](j).toMatrix();
size_t rlIndex = aRlList[i] - 1;
size_t rnIndex = aRnList[j] - 1;
Matrix receiverNormalsMatrix = Recon::receiverNormals[rlIndex](rnIndex ).toMatrix();
Matrix rotateData = rotateAndTranslate(transMatNormals, receiverNormalsMatrix);
size_t rotateSize = rotateData.getDimSize(0);
std::copy(rotateData.getData(), rotateData.getData() + rotateSize, receiverNormalsDataCopyPointer);
receiverNormalsDataCopyPointer += rotateSize;
receiverNormal($,rnIndex,rlIndex) = rotateData;
Matrix receiverPositionsMatrix = Recon::receiverPositions[i](j).toMatrix();
Matrix receiverPositionsMatrix = Recon::receiverPositions[rlIndex](rnIndex).toMatrix();
rotateData = rotateAndTranslate(transMat, receiverPositionsMatrix);
rotateSize = rotateData.getDimSize(0);
std::copy(rotateData.getData(), rotateData.getData() + rotateSize, receiverPositionsCopyPointer);
receiverPositionsCopyPointer += rotateSize;
receiverPosition($,rnIndex,rlIndex) = rotateData;
}
}
aOutput.receiverNormals.push_back(Matrix::fromRawData(receiverNormalsData, 3, aRnList.getDataSize(), aRlList.getDataSize()));
aOutput.receiverPositions.push_back(Matrix::fromRawData(receiverPositionsData, 3, aRnList.getDataSize(), aRlList.getDataSize()));
aOutput.receiverNormals.push_back(receiverNormal);
aOutput.receiverPositions.push_back(receiverPosition);
double* senderNormalsData = new double[senderMatrixSize];
double* senderNormalsDataCopyPointer = senderNormalsData;
double* senderPositionsData = new double[senderMatrixSize];
double* senderPositionsCopyPointer = senderPositionsData;
Matrix senderNormal = Aurora::zeros(3, max(aSnList)[0], max(aSlList)[0]) + NAN;
Matrix senderPosition = Aurora::zeros(3, max(aSnList)[0], max(aSlList)[0]) + NAN;
double maxSl = max(aSlList)[0];
double maxSn = max(aSnList)[0];
for(int i=0; i<aSlList.getDataSize(); ++i)
{
for(int j=0; j<aSnList.getDataSize(); ++j)
{
Matrix senderNormalsMatrix = Recon::emitterNormals[i](j).toMatrix();
size_t slIndex = aSlList[i] - 1;
size_t snIndex = aSnList[j] - 1;
Matrix senderNormalsMatrix = Recon::emitterNormals[slIndex](snIndex).toMatrix();
Matrix rotateData = rotateAndTranslate(transMatNormals, senderNormalsMatrix);
size_t rotateSize = rotateData.getDimSize(0);
std::copy(rotateData.getData(), rotateData.getData() + rotateSize, senderNormalsDataCopyPointer);
senderNormalsDataCopyPointer += rotateSize;
senderNormal($,snIndex,slIndex) = rotateData;
Matrix senderPositionsMatrix = Recon::emitterPositions[i](j).toMatrix();
Matrix senderPositionsMatrix = Recon::emitterPositions[slIndex](snIndex).toMatrix();
rotateData = rotateAndTranslate(transMat, senderPositionsMatrix);
rotateSize = rotateData.getDimSize(0);
std::copy(rotateData.getData(), rotateData.getData() + rotateSize, senderPositionsCopyPointer);
senderPositionsCopyPointer += rotateSize;
senderPosition($,snIndex,slIndex) = rotateData;
}
}
aOutput.senderNormals.push_back(Matrix::fromRawData(senderNormalsData, 3, aSnList.getDataSize(), aSlList.getDataSize()));
aOutput.senderPositions.push_back(Matrix::fromRawData(senderPositionsData, 3, aSnList.getDataSize(), aSlList.getDataSize()));
aOutput.senderNormals.push_back(senderNormal);
aOutput.senderPositions.push_back(senderPosition);
}
}