mirror of
https://github.com/yoshiya-usui/femtic.git
synced 2025-07-20 06:51:12 +08:00
383 lines
14 KiB
C++
383 lines
14 KiB
C++
//-------------------------------------------------------------------------------------------------------
|
|
// The MIT License (MIT)
|
|
//
|
|
// Copyright (c) 2021 Yoshiya Usui
|
|
//
|
|
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
// of this software and associated documentation files (the "Software"), to deal
|
|
// in the Software without restriction, including without limitation the rights
|
|
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
// copies of the Software, and to permit persons to whom the Software is
|
|
// furnished to do so, subject to the following conditions:
|
|
//
|
|
// The above copyright notice and this permission notice shall be included in all
|
|
// copies or substantial portions of the Software.
|
|
//
|
|
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
// SOFTWARE.
|
|
//-------------------------------------------------------------------------------------------------------
|
|
#include "Util.h"
|
|
#include "OutputFiles.h"
|
|
|
|
#include <stdio.h>
|
|
#include <math.h>
|
|
#include <vector>
|
|
#include <assert.h>
|
|
|
|
// Sort elements by its key value with quick sort
|
|
// cf) Numerical Recipes in C++ Second Edition, p336-p339.
|
|
// [Input]:
|
|
// 1) numOfIDs: Number of elements to be sorted
|
|
// 2) values: Values of each element. Elements are sorted by this values.
|
|
// [Input/Output]:
|
|
// 1) ids: Array of elements to be sorted
|
|
void quickSort( const int numOfIDs, int* ids, const double* values ){
|
|
|
|
const int minimumSizeOfQuickSort = 7;
|
|
std::vector< std::pair<int, int> > stack;
|
|
|
|
//const int numOfIDs = sizeof( ids ) / sizeof( ids[0] );
|
|
if( ids == NULL ){
|
|
OutputFiles::m_logFile << "Error : ids is NULL in quickSort !!" << std::endl;
|
|
exit(1);
|
|
}
|
|
if( values == NULL ){
|
|
OutputFiles::m_logFile << "Error : values is NULL quickSort !!" << std::endl;
|
|
exit(1);
|
|
}
|
|
if( numOfIDs <= 0 ){
|
|
OutputFiles::m_logFile << "Error : numOfIDs is equal to or less than zero !!" << std::endl;
|
|
exit(1);
|
|
}
|
|
int iLeft(0);
|
|
int iRight(numOfIDs - 1);
|
|
|
|
while(1){
|
|
|
|
if( iRight - iLeft < minimumSizeOfQuickSort ){
|
|
//----- Insertion sort >>>>>
|
|
for( int j = iLeft + 1; j <= iRight; ++j ){
|
|
const int moved = ids[j];
|
|
const double valueOfElemnentMoved = values[moved];
|
|
int i = j - 1;
|
|
for( ; i >= iLeft; --i ){
|
|
if( values[ids[i]] <= valueOfElemnentMoved ){
|
|
break;
|
|
}
|
|
ids[i+1] = ids[i];
|
|
}
|
|
ids[i+1] = moved;
|
|
}
|
|
|
|
if( stack.empty() ){
|
|
break;// Finish while loop
|
|
}
|
|
iLeft = stack.back().first;
|
|
iRight = stack.back().second;
|
|
stack.pop_back();
|
|
//----- Insertion sort <<<<<
|
|
}else{
|
|
//----- Quick sort >>>>>
|
|
std::swap( ids[(iLeft+iRight)/2] , ids[iLeft+1] );// Exchange element
|
|
if( values[ids[iLeft]] > values[ids[iRight]] ){
|
|
std::swap( ids[iLeft], ids[iRight] );// Exchange element
|
|
}
|
|
if( values[ids[iLeft+1]] > values[ids[iRight]] ){
|
|
std::swap( ids[iLeft+1], ids[iRight] );// Exchange element
|
|
}
|
|
if( values[ids[iLeft]] > values[ids[iLeft+1]] ){
|
|
std::swap( ids[iLeft], ids[iLeft+1] );// Exchange element
|
|
}
|
|
int i = iLeft + 1;
|
|
int j = iRight;
|
|
const int partitioningElement = ids[iLeft+1];// Partitioning element
|
|
const double valueOfPartitioningElement = values[ids[iLeft+1]];// values of partitioning element
|
|
|
|
while(1){
|
|
++i;
|
|
while( values[ids[i]] < valueOfPartitioningElement ){
|
|
++i;// Scan up to find element which >= partitioning element
|
|
}
|
|
--j;
|
|
while( values[ids[j]] > valueOfPartitioningElement ){
|
|
--j;// Scan up to find element which <= partitioning element
|
|
}
|
|
if( j < i ){// i and j are crossed, so that partitioning complete
|
|
break;
|
|
}
|
|
std::swap( ids[i], ids[j] );// Exchange element
|
|
}
|
|
|
|
// Exchange IDs of j and iLeft+1 ( partitioning element )
|
|
ids[iLeft+1] = ids[j];
|
|
ids[j] = partitioningElement;
|
|
|
|
if( iLeft - i + 1 >= j - iLeft ){// Elements in the right side >= Elements in the left side
|
|
stack.push_back( std::pair<int, int>( i, iRight ) );
|
|
iRight = j - 1;
|
|
}else{// Elements in the right side < Elements in the left side
|
|
stack.push_back( std::pair<int, int>( iLeft, j - 1) );
|
|
iLeft = i;
|
|
}
|
|
//----- Quick sort <<<<<
|
|
}
|
|
|
|
}
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
// Sort elements by values of its three keys with quick sort
|
|
// cf) Numerical Recipes in C++ Second Edition, p336-p339.
|
|
// [Input]:
|
|
// 1) numOfIDs: Number of elements to be sorted
|
|
// 2) firstKeyValuesvalues: First key values of each element.
|
|
// 3) secondKeyValues: Second key values of each element.
|
|
// 4) thirdKeyValues: Third key values of each element.
|
|
// [Input/Output]:
|
|
// 1) ids: Array of elements to be sorted
|
|
void quickSortThreeKeys( const int numOfIDs, int* ids,
|
|
const double* firstKeyValues, const double* secondKeyValues, const double* thirdKeyValues ){
|
|
|
|
const double EPSOfValue = 1e-10;
|
|
const int minimumSizeOfQuickSort = 7;
|
|
std::vector< std::pair<int, int> > stack;
|
|
|
|
if( numOfIDs <= 0 ){
|
|
OutputFiles::m_logFile << "Error : numOfIDs is equal to or less than zero in quickSortThreeKeys !!" << std::endl;
|
|
exit(1);
|
|
}
|
|
if( ids == NULL ){
|
|
OutputFiles::m_logFile << "Error : ids is NULL in quickSortThreeKeys !!" << std::endl;
|
|
exit(1);
|
|
}
|
|
if( firstKeyValues == NULL ){
|
|
OutputFiles::m_logFile << "Error : firstKeyValues is NULL quickSortThreeKeys !!" << std::endl;
|
|
exit(1);
|
|
}
|
|
if( secondKeyValues == NULL ){
|
|
OutputFiles::m_logFile << "Error : secondKeyValues is NULL quickSortThreeKeys !!" << std::endl;
|
|
exit(1);
|
|
}
|
|
if( thirdKeyValues == NULL ){
|
|
OutputFiles::m_logFile << "Error : thirdKeyValues is NULL quickSortThreeKeys !!" << std::endl;
|
|
exit(1);
|
|
}
|
|
int iLeft(0);
|
|
int iRight(numOfIDs - 1);
|
|
|
|
while(1){
|
|
|
|
if( iRight - iLeft < minimumSizeOfQuickSort ){
|
|
//----- Insertion sort >>>>>
|
|
for( int j = iLeft + 1; j <= iRight; ++j ){
|
|
const int moved = ids[j];
|
|
//const double valueOfElemnentMoved = values[moved];
|
|
int i = j - 1;
|
|
for( ; i >= iLeft; --i ){
|
|
//if( values[ids[i]] <= valueOfElemnentMoved ){
|
|
// break;
|
|
//}
|
|
//ids[i+1] = ids[i];
|
|
if( compareValueByThreeKeys( ids[i], moved, firstKeyValues, secondKeyValues, thirdKeyValues) == LARGE_LEFT_HAND_SIDE ){
|
|
ids[i+1] = ids[i];
|
|
}else{
|
|
break;
|
|
}
|
|
}
|
|
ids[i+1] = moved;
|
|
}
|
|
|
|
if( stack.empty() ){
|
|
break;// Finish while loop
|
|
}
|
|
iLeft = stack.back().first;
|
|
iRight = stack.back().second;
|
|
stack.pop_back();
|
|
//----- Insertion sort <<<<<
|
|
}else{
|
|
//----- Quick sort >>>>>
|
|
std::swap( ids[(iLeft+iRight)/2] , ids[iLeft+1] );// Exchange element
|
|
//if( values[ids[iLeft]] > values[ids[iRight]] ){
|
|
if( compareValueByThreeKeys( ids[iLeft], ids[iRight], firstKeyValues, secondKeyValues, thirdKeyValues) == LARGE_LEFT_HAND_SIDE ){
|
|
std::swap( ids[iLeft], ids[iRight] );// Exchange element
|
|
}
|
|
//if( values[ids[iLeft+1]] > values[ids[iRight]] ){
|
|
if( compareValueByThreeKeys( ids[iLeft+1], ids[iRight], firstKeyValues, secondKeyValues, thirdKeyValues) == LARGE_LEFT_HAND_SIDE ){
|
|
std::swap( ids[iLeft+1], ids[iRight] );// Exchange element
|
|
}
|
|
//if( values[ids[iLeft]] > values[ids[iLeft+1]] ){
|
|
if( compareValueByThreeKeys( ids[iLeft], ids[iLeft+1], firstKeyValues, secondKeyValues, thirdKeyValues) == LARGE_LEFT_HAND_SIDE ){
|
|
std::swap( ids[iLeft], ids[iLeft+1] );// Exchange element
|
|
}
|
|
int i = iLeft + 1;
|
|
int j = iRight;
|
|
const int partitioningElement = ids[iLeft+1];// Partitioning element
|
|
//const double valueOfPartitioningElement = values[ids[iLeft+1]];// values of partitioning element
|
|
|
|
while(1){
|
|
++i;
|
|
//while( values[ids[i]] < valueOfPartitioningElement ){
|
|
while( compareValueByThreeKeys( ids[i], partitioningElement, firstKeyValues, secondKeyValues, thirdKeyValues) == SMALL_LEFT_HAND_SIDE ){
|
|
++i;// Scan up to find element which >= partitioning element
|
|
}
|
|
--j;
|
|
//while( values[ids[j]] > valueOfPartitioningElement ){
|
|
while( compareValueByThreeKeys( ids[j], partitioningElement, firstKeyValues, secondKeyValues, thirdKeyValues) == LARGE_LEFT_HAND_SIDE ){
|
|
--j;// Scan up to find element which <= partitioning element
|
|
}
|
|
if( j < i ){// i and j are crossed, so that partitioning complete
|
|
break;
|
|
}
|
|
std::swap( ids[i], ids[j] );// Exchange element
|
|
}
|
|
|
|
// Exchange IDs of j and iLeft+1 ( partitioning element )
|
|
ids[iLeft+1] = ids[j];
|
|
ids[j] = partitioningElement;
|
|
|
|
if( iLeft - i + 1 >= j - iLeft ){// Elements in the right side >= Elements in the left side
|
|
stack.push_back( std::pair<int, int>( i, iRight ) );
|
|
iRight = j - 1;
|
|
}else{// Elements in the right side < Elements in the left side
|
|
stack.push_back( std::pair<int, int>( iLeft, j - 1) );
|
|
iLeft = i;
|
|
}
|
|
//----- Quick sort <<<<<
|
|
}
|
|
|
|
}
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
// Compare values by its three keys
|
|
int compareValueByThreeKeys( const int lhsID, const int rhsID,
|
|
const double* firstKeyValues, const double* secondKeyValues, const double* thirdKeyValues ) {
|
|
|
|
const double EPS(1e-10);
|
|
|
|
//----- Compare value by the first key >>>>>
|
|
if( fabs( firstKeyValues[lhsID] - firstKeyValues[rhsID] ) <= EPS ){
|
|
//----- Compare value by the second key >>>>>
|
|
if( fabs( secondKeyValues[lhsID] - secondKeyValues[rhsID] ) <= EPS ){
|
|
//----- Compare value by the third key >>>>>
|
|
if( fabs( thirdKeyValues[lhsID] - thirdKeyValues[rhsID] ) <= EPS ){
|
|
return EQUAL;
|
|
}else if( thirdKeyValues[lhsID] > thirdKeyValues[rhsID] - EPS ){
|
|
return LARGE_LEFT_HAND_SIDE;
|
|
}else{
|
|
return SMALL_LEFT_HAND_SIDE;
|
|
}
|
|
//----- Compare value by the third key <<<<<
|
|
}else if( secondKeyValues[lhsID] > secondKeyValues[rhsID] - EPS ){
|
|
return LARGE_LEFT_HAND_SIDE;
|
|
}else{
|
|
return SMALL_LEFT_HAND_SIDE;
|
|
}
|
|
//----- Compare value by the second key <<<<<
|
|
}else if( firstKeyValues[lhsID] > firstKeyValues[rhsID] - EPS ){
|
|
return LARGE_LEFT_HAND_SIDE;
|
|
}else{
|
|
return SMALL_LEFT_HAND_SIDE;
|
|
}
|
|
//----- Compare value by the first key <<<<<
|
|
|
|
}
|
|
|
|
// Calculate matrix product for 2 x 2 double matrix
|
|
void calcProductFor2x2DoubleMatrix( const CommonParameters::DoubleMatrix2x2& matInA, const CommonParameters::DoubleMatrix2x2& matInB, CommonParameters::DoubleMatrix2x2& matOut ){
|
|
|
|
matOut.comp11 = matInA.comp11 * matInB.comp11 + matInA.comp12 * matInB.comp21;
|
|
|
|
#ifdef _DEBUG_WRITE
|
|
std::cout << "matOut.comp11 matInA.comp11 matInB.comp11 matInA.comp12 matInB.comp21 : " << matOut.comp11 << " " << matInA.comp11 << " " << matInB.comp11 << " " << matInA.comp12 << " " << matInB.comp21 << std::endl;
|
|
#endif
|
|
|
|
matOut.comp12 = matInA.comp11 * matInB.comp12 + matInA.comp12 * matInB.comp22;
|
|
matOut.comp21 = matInA.comp21 * matInB.comp11 + matInA.comp22 * matInB.comp21;
|
|
matOut.comp22 = matInA.comp21 * matInB.comp12 + matInA.comp22 * matInB.comp22;
|
|
|
|
return;
|
|
}
|
|
|
|
// Calculate 3D vectors
|
|
CommonParameters::Vector3D calcVector3D( const CommonParameters::locationXYZ& startCoords, const CommonParameters::locationXYZ& endCoords ){
|
|
|
|
const CommonParameters::Vector3D vec = { endCoords.X - startCoords.X, endCoords.Y - startCoords.Y, endCoords.Z - startCoords.Z };
|
|
return vec;
|
|
|
|
}
|
|
|
|
// Calculate outer product of 3D vectors
|
|
CommonParameters::Vector3D calcOuterProduct( const CommonParameters::Vector3D& vec1, const CommonParameters::Vector3D& vec2 ){
|
|
|
|
CommonParameters::Vector3D result = { vec1.Y*vec2.Z - vec1.Z*vec2.Y, vec1.Z*vec2.X - vec1.X*vec2.Z, vec1.X*vec2.Y - vec1.Y*vec2.X };
|
|
|
|
return result;
|
|
|
|
}
|
|
|
|
// Calculate inner product of 3D vectors
|
|
double calcInnerProduct( const CommonParameters::Vector3D& vec1, const CommonParameters::Vector3D& vec2 ){
|
|
|
|
return vec1.X*vec2.X + vec1.Y*vec2.Y + vec1.Z*vec2.Z;
|
|
|
|
}
|
|
|
|
// Calculate impedance tensor component from apparent resisitivity and phase
|
|
void calcImpedanceTensorComponentFromApparentResistivityAndPhase( const double freq, const double appRes, const double appResError,
|
|
const double phase, const double phaseError,
|
|
std::complex<double>& Z, CommonParameters::DoubleComplexValues& ZError ){
|
|
|
|
assert( appResError > 0 );
|
|
assert( phaseError > 0 );
|
|
const double omega = 2.0 * CommonParameters::PI * freq;
|
|
const double absZ = sqrt( appRes * CommonParameters::mu * omega );
|
|
const double phaseRad = phase * CommonParameters::deg2rad;
|
|
Z = std::complex<double>( absZ * cos(phaseRad), absZ * sin(phaseRad) );
|
|
|
|
ZError.realPart = hypot( 0.5 * sqrt(CommonParameters::mu * omega / appRes) * cos(phaseRad) * appResError, absZ * sin(phaseRad) * CommonParameters::deg2rad * phaseError );
|
|
ZError.imagPart = hypot( 0.5 * sqrt(CommonParameters::mu * omega / appRes) * sin(phaseRad) * appResError, absZ * cos(phaseRad) * CommonParameters::deg2rad * phaseError );
|
|
// 2 * (d|Z|/drhoa) * drhoa = |Z| * drhoa / rhoa
|
|
if( ZError.realPart > absZ * appResError / appRes ){
|
|
ZError.realPart = absZ * appResError / appRes;
|
|
}
|
|
if( ZError.imagPart > absZ * appResError / appRes ){
|
|
ZError.imagPart = absZ * appResError / appRes;
|
|
}
|
|
|
|
}
|
|
|
|
#ifdef _ANISOTOROPY
|
|
void rotateTensor( double tensorRotated[3][3], const double rotationTensor[3][3] ){
|
|
|
|
double tempTensor[3][3];
|
|
for( int row = 0; row < 3; ++row ){
|
|
for( int col = 0; col < 3; ++col ){
|
|
tempTensor[row][col] = 0.0;
|
|
for( int i = 0; i < 3; ++i ){
|
|
tempTensor[row][col] += rotationTensor[i][row] * tensorRotated[i][col];
|
|
}
|
|
}
|
|
}
|
|
|
|
for( int row = 0; row < 3; ++row ){
|
|
for( int col = 0; col < 3; ++col ){
|
|
tensorRotated[row][col] = 0.0;
|
|
for( int i = 0; i < 3; ++i ){
|
|
tensorRotated[row][col] += tempTensor[row][i] * rotationTensor[i][col];
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
#endif
|