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