void
PlaneGeometry::EnsurePerpendicularNormal(mitk::AffineTransform3D *transform)
{
//ensure row(2) of transform to be perpendicular to plane, keep length.
VnlVector normal = vnl_cross_3d(
transform->GetMatrix().GetVnlMatrix().get_column(0),
transform->GetMatrix().GetVnlMatrix().get_column(1) );
normal.normalize();
ScalarType len = transform->GetMatrix()
.GetVnlMatrix().get_column(2).two_norm();
if (len==0) len = 1;
normal*=len;
Matrix3D matrix = transform->GetMatrix();
matrix.GetVnlMatrix().set_column(2, normal);
transform->SetMatrix(matrix);
}
// Prediction for a single ROI
// Accepts an arbitrary number of integral images/channels.
// Assumes that predPtr is already allocated, of same size as imgPtr
// Returns 0 if ok
DLL_EXPORT
int predictWithChannels( void *modelPtr, ImagePixelType *imgPtr,
void *eigVecImgPtr,
int width, int height, int depth,
IntegralImagePixelType **chImgPtr,
int numChannels, double zAnisotropyFactor,
int useEarlyStopping,
PredictionPixelType *predPtr )
{
Matrix3D<PredictionPixelType> predMatrix;
predMatrix.fromSharedData( predPtr, width, height, depth );
// create roi for image, no GT available
ROIData roi;
roi.init( imgPtr, 0, 0, 0, width, height, depth, zAnisotropyFactor, 0.0, (const ROIData::RotationMatrixType *) eigVecImgPtr );
std::unique_ptr<ROIData::IntegralImageType[]> ii(new ROIData::IntegralImageType[numChannels]); // TODO: remove
for (int ch=0; ch < numChannels; ch++)
{
ii[ch].fromSharedData(chImgPtr[ch], width, height, depth);
roi.addII( ii[ch].internalImage().data() );
}
MultipleROIData allROIs;
allROIs.add( shared_ptr_nodelete(ROIData, &roi) );
try
{
Booster adaboost;
adaboost.setModel( *((BoosterModel *) modelPtr) );
if(useEarlyStopping != 0)
adaboost.predictWithFeatureOrdering<true>( allROIs, &predMatrix );
else
adaboost.predictWithFeatureOrdering<false>( allROIs, &predMatrix );
}
catch( std::exception &e )
{
printf("Error in prediction: %s\n", e.what());
return -1;
}
return 0;
}
/*** Eigenvectors of Image wrappers ****/
DLL_EXPORT
void *computeEigenVectorsOfHessianImage( ImagePixelType *imgPtr,
int width, int height, int depth,
double zAnisotropyFactor,
double sigma )
{
Matrix3D<ImagePixelType> rawImg;
rawImg.fromSharedData( imgPtr, width, height, depth );
// compute eigen stuff
ROIData::ItkEigenVectorImageType::Pointer rotImg =
AllEigenVectorsOfHessian::allEigenVectorsOfHessian<ImagePixelType>(
sigma, zAnisotropyFactor, rawImg.asItkImage(),
AllEigenVectorsOfHessian::EByMagnitude );
rotImg->GetPixelContainer()->SetContainerManageMemory(false);
return rotImg->GetPixelContainer()->GetImportPointer();
}
void Matrix3DUtils::getRotation(const Matrix3D &m, Vector3D &out) {
Vector3D *v = new Vector3D[3]();
m.decompose(Matrix3D::Style::EulerAngles, v);
_vector.copyFrom(v[1]);
out.x = _vector.x * _toAng;
out.y = _vector.y * _toAng;
out.z = _vector.z * _toAng;
// free v
delete [] v;
}
void
PlaneGeometry::SetMatrixByVectors( const VnlVector &rightVector,
const VnlVector &downVector, ScalarType thickness /* = 1.0 */ )
{
VnlVector normal = vnl_cross_3d(rightVector, downVector);
normal.normalize();
normal *= thickness;
// Crossproduct vnl_cross_3d is always righthanded, but that is okay here
// because in this method we create a new IndexToWorldTransform and
// a negative thickness could still make it lefthanded.
AffineTransform3D::Pointer transform = AffineTransform3D::New();
Matrix3D matrix;
matrix.GetVnlMatrix().set_column(0, rightVector);
matrix.GetVnlMatrix().set_column(1, downVector);
matrix.GetVnlMatrix().set_column(2, normal);
transform->SetMatrix(matrix);
transform->SetOffset(this->GetIndexToWorldTransform()->GetOffset());
SetIndexToWorldTransform(transform);
}
void set(int id, Figure *fig, Texture *tex, Matrix3D *mtx, GCObject *userObj) {
if (mtx) {mtx->retain();}
if (fig) {fig->retain();}
if (tex) {tex->retain();}
if (userObj) {userObj->retain();}
this->id = id;
this->fig = fig;
this->tex = tex;
this->mtx = mtx;
this->userObj = userObj;
}
unsigned int mitk::PlanePositionManagerService::AddNewPlanePosition ( const Geometry2D* plane, unsigned int sliceIndex )
{
for (unsigned int i = 0; i < m_PositionList.size(); ++i)
{
if (m_PositionList[i] != 0)
{
bool isSameMatrix(true);
bool isSameOffset(true);
isSameOffset = mitk::Equal(m_PositionList[i]->GetTransform()->GetOffset(), plane->GetIndexToWorldTransform()->GetOffset());
if(!isSameOffset || sliceIndex != m_PositionList[i]->GetPos())
continue;
isSameMatrix = mitk::MatrixEqualElementWise(m_PositionList[i]->GetTransform()->GetMatrix(), plane->GetIndexToWorldTransform()->GetMatrix());
if(isSameMatrix)
return i;
}
}
AffineTransform3D::Pointer transform = AffineTransform3D::New();
Matrix3D matrix;
matrix.GetVnlMatrix().set_column(0, plane->GetIndexToWorldTransform()->GetMatrix().GetVnlMatrix().get_column(0));
matrix.GetVnlMatrix().set_column(1, plane->GetIndexToWorldTransform()->GetMatrix().GetVnlMatrix().get_column(1));
matrix.GetVnlMatrix().set_column(2, plane->GetIndexToWorldTransform()->GetMatrix().GetVnlMatrix().get_column(2));
transform->SetMatrix(matrix);
transform->SetOffset(plane->GetIndexToWorldTransform()->GetOffset());
mitk::Vector3D direction;
direction[0] = plane->GetIndexToWorldTransform()->GetMatrix().GetVnlMatrix().get_column(2)[0];
direction[1] = plane->GetIndexToWorldTransform()->GetMatrix().GetVnlMatrix().get_column(2)[1];
direction[2] = plane->GetIndexToWorldTransform()->GetMatrix().GetVnlMatrix().get_column(2)[2];
direction.Normalize();
mitk::RestorePlanePositionOperation* newOp = new mitk::RestorePlanePositionOperation (OpRESTOREPLANEPOSITION, plane->GetExtent(0),
plane->GetExtent(1), plane->GetSpacing(), sliceIndex, direction, transform);
m_PositionList.push_back( newOp );
return GetNumberOfPlanePositions()-1;
}
void Constraint::StandardJointDrawing(Matrix3D& A, Vector3D& p, Vector3D& d_dir, Vector3D& d_rot, int flag, double draw_factor)
{
//double draw_factor = GetDrawSizeScalar();
Vector3D draw_dir;
for (int i=1; i<=3; i++)
{
draw_dir = draw_factor*Vector3D(A.Get0(0,i-1),A.Get0(1,i-1),A.Get0(2,i-1));
if(d_dir(i)!=0)
{
mbs->SetColor(GetCol());
mbs->DrawCone(p + flag*draw_dir,p,draw_factor*1.1,6,1);
}
if(d_rot(i)!=0)
{
//GetMBS()->ChooseColor(0.0f,0.6f,0.0f);
mbs->SetColor(GetColExt());
mbs->DrawCone(p + 2*flag*draw_dir,p,draw_factor,6,1);
mbs->DrawCone(p + 4*flag*draw_dir,p + 2*flag*draw_dir,draw_factor,6,1);
//mbs->SetColor(GetCol());
}
}
}
// Prediction
// Internally computes integral image from raw image, thus limited functionality.
// Assumes that predPtr is already allocated, of same size as imgPtr.
DLL_EXPORT
void predict( void *modelPtr, ImagePixelType *imgPtr, int width, int height, int depth, PredictionPixelType *predPtr )
{
Matrix3D<PredictionPixelType> predMatrix;
predMatrix.fromSharedData( predPtr, width, height, depth );
// create roi for image, no GT available
ROIData roi;
roi.init( imgPtr, 0, 0, 0, width, height, depth );
// raw image to integral image
ROIData::IntegralImageType ii;
ii.compute( roi.rawImage );
roi.addII( ii.internalImage().data() );
MultipleROIData allROIs;
allROIs.add( shared_ptr_nodelete(ROIData, &roi) );
Booster adaboost;
adaboost.setModel( *((BoosterModel *) modelPtr) );
adaboost.predict<false>( allROIs, &predMatrix );
}
void
PlaneGeometry::InitializeStandardPlane( mitk::ScalarType width, ScalarType height,
const VnlVector &rightVector, const VnlVector &downVector,
const Vector3D *spacing )
{
assert(width > 0);
assert(height > 0);
VnlVector rightDV = rightVector; rightDV.normalize();
VnlVector downDV = downVector; downDV.normalize();
VnlVector normal = vnl_cross_3d(rightVector, downVector);
normal.normalize();
// Crossproduct vnl_cross_3d is always righthanded, but that is okay here
// because in this method we create a new IndexToWorldTransform and
// spacing with 1 or 3 negative components could still make it lefthanded.
if(spacing!=nullptr)
{
rightDV *= (*spacing)[0];
downDV *= (*spacing)[1];
normal *= (*spacing)[2];
}
AffineTransform3D::Pointer transform = AffineTransform3D::New();
Matrix3D matrix;
matrix.GetVnlMatrix().set_column(0, rightDV);
matrix.GetVnlMatrix().set_column(1, downDV);
matrix.GetVnlMatrix().set_column(2, normal);
transform->SetMatrix(matrix);
transform->SetOffset(this->GetIndexToWorldTransform()->GetOffset());
ScalarType bounds[6] = { 0, width, 0, height, 0, 1 };
this->SetBounds( bounds );
this->SetIndexToWorldTransform( transform );
}
void DirectionalLight::getObjectProjectionMatrix(IRenderable* renderable, Camera3D* camera, Matrix3D& result)
{
Matrix3D matrix(renderable->getRenderSceneTransform(camera));
matrix.append(getInverseSceneTransform());
float projAABBPoints[24];
BoundingVolumeBase* bounds = renderable->getSourceEntity()->getBounds();
matrix.transformVectors(bounds->getAABBPoints(), projAABBPoints, 8);
float xMin, yMin, zMin, xMax, yMax, zMax;
xMin = yMin = zMin = MathConsts::Infinity;
xMax = yMax = zMax = -MathConsts::Infinity;
for (int i = 0; i < 24;)
{
float d = projAABBPoints[i++];
if (d < xMin)
xMin = d;
if (d > xMax)
xMax = d;
d = projAABBPoints[i++];
if (d < yMin)
yMin = d;
if (d > yMax)
yMax = d;
d = projAABBPoints[i++];
if (d < zMin)
zMin = d;
if (d > zMax)
zMax = d;
}
float invXRange = 1 / (xMax - xMin);
float invYRange = 1 / (yMax - yMin);
float invZRange = 1 / (zMax - zMin);
float(&raw)[16] = result.m_rawData;
raw[0] = 2 * invXRange;
raw[5] = 2 * invYRange;
raw[10] = invZRange;
raw[12] = -(xMax + xMin) * invXRange;
raw[13] = -(yMax + yMin) * invYRange;
raw[14] = -zMin * invZRange;
raw[1] = raw[2] = raw[3] = raw[4] = raw[6] = raw[7] = raw[8] = raw[9] = raw[11] = 0;
raw[15] = 1;
result.prepend(matrix);
}
void Reader::computeRT(Matrix3D & R, Vector3D & t, const Vector3D & cam_dir,
const Vector3D & cam_pos, const Vector3D & cam_up)
{
Vector3D x, y, z;
z = cam_dir / length(cam_dir);
x = cross(cam_up, z);
x = normalize(x);
y = cross(z, x);
R = Matrix3D(x, y, z);
R = R.trans();
t = cam_pos;
}
void SplineUtils::extractBasis (const Go::BasisDerivsSf2& spline,
Vector& N, Matrix& dNdu, Matrix3D& d2Ndu2)
{
N .resize(spline.basisValues.size());
dNdu .resize(N.size(),2);
d2Ndu2.resize(N.size(),2,2);
size_t jp, n = 1;
for (jp = 0; jp < N.size(); jp++, n++)
{
N (n) = spline.basisValues[jp];
dNdu (n,1) = spline.basisDerivs_u[jp];
dNdu (n,2) = spline.basisDerivs_v[jp];
d2Ndu2(n,1,1) = spline.basisDerivs_uu[jp];
d2Ndu2(n,1,2) = d2Ndu2(n,2,1) = spline.basisDerivs_uv[jp];
d2Ndu2(n,2,2) = spline.basisDerivs_vv[jp];
}
}
void ASMs1D::extractBasis (double u, Vector& N, Matrix& dNdu,
Matrix3D& d2Ndu2) const
{
int p1 = curv->order();
RealArray bas(p1*3);
curv->basis().computeBasisValues(u,&bas.front(),2);
N.resize(p1);
dNdu.resize(p1,1);
d2Ndu2.resize(p1,1,1);
for (int i = 1; i <= p1; i++)
{
N(i) = bas[3*i-3];
dNdu(i,1) = bas[3*i-2];
d2Ndu2(i,1,1) = bas[3*i-1];
}
}
//critical function here - must be optimized for speed
//int Solution::getSubgroupsMatrix(Rules& r, qint8 a[MAX_TOTAL_SUBGROUPS][MAX_DAYS_PER_WEEK][MAX_HOURS_PER_DAY]){
int Solution::getSubgroupsMatrix(Rules& r, Matrix3D<qint8>& a){
assert(r.initialized);
assert(r.internalStructureComputed);
int conflicts=0;
a.resize(r.nInternalSubgroups, r.nDaysPerWeek, r.nHoursPerDay);
int i;
for(i=0; i<r.nInternalSubgroups; i++)
for(int j=0; j<r.nDaysPerWeek; j++)
for(int k=0; k<r.nHoursPerDay; k++)
a[i][j][k]=0;
for(i=0; i<r.nInternalActivities; i++)
if(this->times[i]!=UNALLOCATED_TIME){
int hour=this->times[i]/r.nDaysPerWeek;
int day=this->times[i]%r.nDaysPerWeek;
Activity* act = &r.internalActivitiesList[i];
for(int dd=0; dd < act->duration && hour+dd < r.nHoursPerDay; dd++)
for(int isg=0; isg < act->iSubgroupsList.count(); isg++){ //isg => index subgroup
int sg = act->iSubgroupsList.at(isg); //sg => subgroup
int tmp=a[sg][day][hour+dd];
/*if(act->parity == PARITY_WEEKLY){
conflicts += tmp<2 ? tmp : 2;
a[sg][day][hour+dd]+=2;
}
else{
assert(act->parity == PARITY_FORTNIGHTLY);
conflicts += tmp<2 ? 0 : 1;
a[sg][day][hour+dd]++;
}*/
conflicts += tmp==0 ? 0 : 1;
a[sg][day][hour+dd]++;
}
}
this->changedForMatrixCalculation=false;
return conflicts;
}
void Object3D::setTransform(Matrix3D& value)
{
//ridiculous matrix error
if (value.m_rawData[0] == 0)
value.m_rawData[0] = 0.0000000000000000000001f;
Vector3D elements[3];
value.decompose(elements);
Vector3D& vec = elements[0];
if (m_position.m_x != vec.m_x || m_position.m_y != vec.m_y || m_position.m_z != vec.m_z)
{
m_position.m_x = vec.m_x;
m_position.m_y = vec.m_y;
m_position.m_z = vec.m_z;
invalidatePosition();
}
vec = elements[1];
if (m_rotation.m_x != vec.m_x || m_rotation.m_y != vec.m_y || m_rotation.m_z != vec.m_z)
{
m_rotation.m_x = vec.m_x;
m_rotation.m_y = vec.m_y;
m_rotation.m_z = vec.m_z;
invalidateRotation();
}
vec = elements[2];
if (m_scaling.m_x != vec.m_x || m_scaling.m_y != vec.m_y || m_scaling.m_z != vec.m_z)
{
m_scaling.m_x = vec.m_x;
m_scaling.m_y = vec.m_y;
m_scaling.m_z = vec.m_z;
invalidateScale();
}
}
// get orthogonalization matrix
const Matrix3D &CrystalInfo::getOrthMat() const
{
Matrix3D retval;
if (m_pOrthMat!=NULL) {
return *m_pOrthMat;
}
double alpha = m_alpha*M_PI/180;
double beta = m_beta*M_PI/180;
double gamma = m_gamma*M_PI/180;
double coaster, siaster;
coaster =
(cos(beta)*cos(gamma) - cos(alpha))/
(sin(beta)*sin(gamma));
siaster = sqrt(1-coaster*coaster);
retval.aij(1, 1) = m_cella;
retval.aij(1, 2) = m_cellb * cos(gamma);
retval.aij(1, 3) = m_cellc * cos(beta);
retval.aij(2, 1) = 0.0;
retval.aij(2, 2) = m_cellb * sin(gamma);
retval.aij(2, 3) = -m_cellc * sin(beta)*coaster;
retval.aij(3, 1) = 0.0;
retval.aij(3, 2) = 0.0;
retval.aij(3, 3) = m_cellc * sin(beta)*siaster;
CrystalInfo *pthis = (CrystalInfo *)this;
pthis->m_pOrthMat = new Matrix3D(retval);
// retval.dump();
return *m_pOrthMat;
}
Matrix3D Matrix3D::_CreateTranslateAlongWorldRight(double fAmt) {
Matrix3D result;
result._SetIdentityMatrix();
result._POSITION_X = fAmt;
return result;
}
void PMeanPMetricTest::test_hessian()
{
MsqError err;
FauxMetric m;
ElementPMeanP m1( 1.0, &m );
ElementPMeanP m2( 2.0, &m );
// get vertices for later
std::vector<size_t> vertices;
pd.element_by_index(0).get_vertex_indices( vertices );
// evaluate gradient
double v1, v2, v3, v4;
std::vector<size_t> indices1, indices2, indices3, indices4, tmpi;
std::vector<Vector3D> grad1, grad2, grad3, grad4;
std::vector<Matrix3D> hess3, hess4;
m1.evaluate_with_gradient( pd, 0, v1, indices1, grad1, err );
CPPUNIT_ASSERT(!err);
m2.evaluate_with_gradient( pd, 0, v2, indices2, grad2, err );
CPPUNIT_ASSERT(!err);
// evaluate with Hessian
m1.evaluate_with_Hessian( pd, 0, v3, indices3, grad3, hess3, err );
CPPUNIT_ASSERT(!err);
m2.evaluate_with_Hessian( pd, 0, v4, indices4, grad4, hess4, err );
CPPUNIT_ASSERT(!err);
// compare value and indices to eval w/out gradient
CPPUNIT_ASSERT_DOUBLES_EQUAL( v1, v3, 1e-6 );
CPPUNIT_ASSERT_DOUBLES_EQUAL( v2, v4, 1e-6 );
// It isn't a requirement that the index order remain the same
// for both eval_with_grad and eval_with_Hess, but assuming it
// does simplifies a lot of stuff in this test. Check that the
// assumption remains valid.
CPPUNIT_ASSERT( indices3 == indices1 );
CPPUNIT_ASSERT( indices4 == indices2 );
// It isn't a requirement that the index order remain the same
// for any value of P, but assuming it does simplifies a lot
// of stuff in this test, so check that the assumption is valid.
CPPUNIT_ASSERT( indices1 == indices2 );
// check that gradient values match
for (size_t i = 0; i < indices1.size(); ++i) {
CPPUNIT_ASSERT_VECTORS_EQUAL( grad1[i], grad3[i], 1e-5 );
CPPUNIT_ASSERT_VECTORS_EQUAL( grad2[i], grad4[i], 1e-5 );
}
// setup evaluation of underying metric
std::vector<size_t> handles;
m.get_element_evaluations( pd, 0, handles, err );
CPPUNIT_ASSERT(!err);
// calculate expected Hessians
std::vector<Vector3D> g;
std::vector<Matrix3D> expected1, expected2, h;
std::vector<Matrix3D>::iterator h_iter;
const unsigned N = vertices.size();
expected1.resize( N*(N+1)/2, Matrix3D(0,0,0,0,0,0,0,0,0) );
expected2 = expected1;
Matrix3D outer;
for (unsigned i = 0; i < handles.size(); ++i) {
double v;
m.evaluate_with_Hessian( pd, handles[i], v, tmpi, g, h, err );
CPPUNIT_ASSERT(!err);
h_iter = h.begin();
for (unsigned r = 0; r < tmpi.size(); ++r) {
unsigned R = index_of( vertices, tmpi[r] );
CPPUNIT_ASSERT( R < N );
for (unsigned c = r; c < tmpi.size(); ++c ,++h_iter) {
unsigned C = index_of( vertices, tmpi[c] );
CPPUNIT_ASSERT( C < N );
if (R <= C) {
unsigned idx = N*R - R*(R+1)/2 + C;
expected1[idx] += 1.0 / handles.size() * *h_iter;
expected2[idx] += 2.0 * v / handles.size() * *h_iter;
outer.outer_product( g[r], g[c] );
expected2[idx] += 2.0 / handles.size() * outer;
}
else {
unsigned idx = N*C - C*(C+1)/2 + R;
expected1[idx] += 1.0 / handles.size() * transpose(*h_iter);
expected2[idx] += 2.0 * v / handles.size() * transpose(*h_iter);
outer.outer_product( g[c], g[r] );
expected2[idx] += 2.0 / handles.size() * outer;
}
}
}
}
// compare Hessians
unsigned H_idx = 0;
for (unsigned R = 0; R < vertices.size(); ++R) {
if (vertices[R] >= pd.num_free_vertices())
continue;
unsigned r = index_of( indices3, vertices[R] );
CPPUNIT_ASSERT(r < indices3.size() );
for (unsigned C = R; C < vertices.size(); ++C, ++H_idx) {
if (vertices[C] >= pd.num_free_vertices())
continue;
unsigned c = index_of( indices3, vertices[C] );
CPPUNIT_ASSERT( c < indices3.size() );
if (r <= c) {
unsigned idx = indices3.size()*r - r*(r+1)/2 + c;
CPPUNIT_ASSERT_MATRICES_EQUAL( expected1[H_idx], hess3[idx], 1e-5 );
CPPUNIT_ASSERT_MATRICES_EQUAL( expected2[H_idx], hess4[idx], 1e-5 );
}
else {
unsigned idx = indices3.size()*c - c*(c+1)/2 + r;
CPPUNIT_ASSERT_MATRICES_EQUAL( transpose(expected1[H_idx]), hess3[idx], 1e-5 );
CPPUNIT_ASSERT_MATRICES_EQUAL( transpose(expected2[H_idx]), hess4[idx], 1e-5 );
}
}
}
}
void
PlaneGeometry::ExecuteOperation( Operation *operation )
{
vtkTransform *transform = vtkTransform::New();
transform->SetMatrix( m_VtkMatrix );
switch ( operation->GetOperationType() )
{
case OpORIENT:
{
mitk::PlaneOperation *planeOp = dynamic_cast< mitk::PlaneOperation * >( operation );
if ( planeOp == NULL )
{
return;
}
Point3D center = planeOp->GetPoint();
Vector3D orientationVector = planeOp->GetNormal();
Vector3D defaultVector;
FillVector3D( defaultVector, 0.0, 0.0, 1.0 );
Vector3D rotationAxis = itk::CrossProduct( orientationVector, defaultVector );
//vtkFloatingPointType rotationAngle = acos( orientationVector[2] / orientationVector.GetNorm() );
vtkFloatingPointType rotationAngle = atan2( (double) rotationAxis.GetNorm(), (double) (orientationVector * defaultVector) );
rotationAngle *= 180.0 / vnl_math::pi;
transform->PostMultiply();
transform->Identity();
transform->Translate( center[0], center[1], center[2] );
transform->RotateWXYZ( rotationAngle, rotationAxis[0], rotationAxis[1], rotationAxis[2] );
transform->Translate( -center[0], -center[1], -center[2] );
break;
}
case OpRESTOREPLANEPOSITION:
{
RestorePlanePositionOperation *op = dynamic_cast< mitk::RestorePlanePositionOperation* >(operation);
if(op == NULL)
{
return;
}
AffineTransform3D::Pointer transform2 = AffineTransform3D::New();
Matrix3D matrix;
matrix.GetVnlMatrix().set_column(0, op->GetTransform()->GetMatrix().GetVnlMatrix().get_column(0));
matrix.GetVnlMatrix().set_column(1, op->GetTransform()->GetMatrix().GetVnlMatrix().get_column(1));
matrix.GetVnlMatrix().set_column(2, op->GetTransform()->GetMatrix().GetVnlMatrix().get_column(2));
transform2->SetMatrix(matrix);
Vector3D offset = op->GetTransform()->GetOffset();
transform2->SetOffset(offset);
this->SetIndexToWorldTransform(transform2);
ScalarType bounds[6] = {0, op->GetWidth(), 0, op->GetHeight(), 0 ,1 };
this->SetBounds(bounds);
TransferItkToVtkTransform();
this->Modified();
transform->Delete();
return;
}
default:
Superclass::ExecuteOperation( operation );
transform->Delete();
return;
}
m_VtkMatrix->DeepCopy(transform->GetMatrix());
this->TransferVtkToItkTransform();
this->Modified();
transform->Delete();
}
bool MultiplyQualityMetric::evaluate_with_Hessian( PatchData& pd,
size_t handle,
double& value,
std::vector<size_t>& indices,
std::vector<Vector3D>& gradient,
std::vector<Matrix3D>& Hessian,
MsqError& err )
{
std::vector<size_t>::iterator i;
size_t j, r, c, n, h;
double val1, val2;
bool rval1, rval2;
rval1 = metric1.evaluate_with_Hessian( pd, handle, val1, indices1, grad1, Hess1, err ); MSQ_ERRZERO(err);
rval2 = metric2.evaluate_with_Hessian( pd, handle, val2, indices2, grad2, Hess2, err ); MSQ_ERRZERO(err);
// merge index lists
indices.resize( indices1.size() + indices2.size() );
i = std::copy( indices1.begin(), indices1.end(), indices.begin() );
std::copy( indices2.begin(), indices2.end(), i );
std::sort( indices.begin(), indices.end() );
indices.erase( std::unique( indices.begin(), indices.end() ), indices.end() );
// calculate grads and convert index lists to indices into output list
gradient.clear();
gradient.resize( indices.size(), Vector3D(0.0) );
for (j = 0; j < indices1.size(); ++j)
{
i = std::lower_bound( indices.begin(), indices.end(), indices1[j] );
indices1[j] = i - indices.begin();
gradient[indices1[j]] += val2 * grad1[j];
}
for (j = 0; j < indices2.size(); ++j)
{
i = std::lower_bound( indices.begin(), indices.end(), indices2[j] );
indices2[j] = i - indices.begin();
gradient[indices2[j]] += val1 * grad2[j];
}
// allocate space for hessians, and zero it
const size_t N = indices.size();
Hessian.clear();
Hessian.resize( N * (N+1) / 2, Matrix3D(0.0) );
// add hessian terms from first metric
n = indices1.size();
h = 0;
for (r = 0; r < n; ++r)
{
const size_t nr = indices1[r];
for (c = r; c < n; ++c)
{
const size_t nc = indices1[c];
Hess1[h] *= val2;
if (nr <= nc)
Hessian[N*nr - nr*(nr+1)/2 + nc] += Hess1[h];
else
Hessian[N*nc - nc*(nc+1)/2 + nr].plus_transpose_equal( Hess1[h] );
++h;
}
}
// add hessian terms from second metric
n = indices2.size();
h = 0;
for (r = 0; r < n; ++r)
{
const size_t nr = indices2[r];
for (c = r; c < n; ++c)
{
const size_t nc = indices2[c];
Hess2[h] *= val1;
if (nr <= nc)
Hessian[N*nr - nr*(nr+1)/2 + nc] += Hess2[h];
else
Hessian[N*nc - nc*(nc+1)/2 + nr].plus_transpose_equal( Hess2[h] );
++h;
}
}
// add gradient outer products
n = indices1.size();
size_t m = indices2.size();
Matrix3D outer;
for (r = 0; r < n; ++r)
{
const size_t nr = indices1[r];
for (c = 0; c < m; ++c)
{
const size_t nc = indices2[c];
outer.outer_product( grad1[r], grad2[c] );
if (nr == nc)
Hessian[N*nr - nr*(nr+1)/2 + nc] += outer.plus_transpose_equal(outer);
else if (nr < nc)
Hessian[N*nr - nr*(nr+1)/2 + nc] += outer;
else
Hessian[N*nc - nc*(nc+1)/2 + nr].plus_transpose_equal(outer);
}
}
value = val1 * val2;
return rval1 && rval2;
}
bool PMeanPTemplate::evaluate_with_Hessian( EvalType type,
PatchData& pd,
double& value_out,
std::vector<Vector3D>& grad_out,
MsqHessian& Hessian_out,
MsqError& err )
{
QualityMetric* qm = get_quality_metric();
qm->get_evaluations( pd, qmHandles, OF_FREE_EVALS_ONLY, err ); MSQ_ERRFALSE(err);
// zero gradient and hessian
grad_out.clear();
grad_out.resize( pd.num_free_vertices(), 0.0 );
Hessian_out.zero_out();
// calculate OF value and gradient for just the patch
std::vector<size_t>::const_iterator i;
size_t j, k, n;
double value, working_sum = 0.0;
const double f1 = qm->get_negate_flag() * mPower.value();
const double f2 = f1 * (mPower.value() - 1);
Matrix3D m;
for (i = qmHandles.begin(); i != qmHandles.end(); ++i)
{
bool result = qm->evaluate_with_Hessian( pd, *i, value, mIndices, mGradient, mHessian, err );
if (MSQ_CHKERR(err) || !result)
return false;
if (fabs(value) < DBL_EPSILON)
continue;
const size_t nfree = mIndices.size();
n = 0;
if (mPower.value() == 1.0) {
working_sum += mPower.raise( value );
for (j = 0; j < nfree; ++j) {
mGradient[j] *= f1;
grad_out[mIndices[j]] += mGradient[j];
for (k = j; k < nfree; ++k) {
mHessian[n] *= f1;
Hessian_out.add( mIndices[j], mIndices[k], mHessian[n], err ); MSQ_ERRFALSE(err);
++n;
}
}
}
else {
const double r2 = mPowerMinus2.raise( value );
const double r1 = r2 * value;
working_sum += r1 * value;
const double hf = f2 * r2;
const double gf = f1 * r1;
for (j = 0; j < nfree; ++j) {
for (k = j; k < nfree; ++k) {
m.outer_product( mGradient[j], mGradient[k] );
m *= hf;
mHessian[n] *= gf;
m += mHessian[n];
Hessian_out.add( mIndices[j], mIndices[k], m, err ); MSQ_ERRFALSE(err);
++n;
}
}
for (j = 0; j < nfree; ++j) {
mGradient[j] *= gf;
grad_out[mIndices[j]] += mGradient[j];
}
}
}
// get overall OF value, update member data, etc.
size_t global_count;
value_out = qm->get_negate_flag()
* get_value( working_sum, qmHandles.size(), type, global_count );
const double inv_n = 1.0 / global_count;
std::vector<Vector3D>::iterator g;
for (g = grad_out.begin(); g != grad_out.end(); ++g)
*g *= inv_n;
Hessian_out.scale( inv_n );
return true;
}
//The following 2 functions (GetTeachersTimetable & GetSubgroupsTimetable)
//are very similar to the above 2 ones (GetTeachersMatrix & GetSubgroupsMatrix)
//void Solution::getTeachersTimetable(Rules& r, qint16 a[MAX_TEACHERS][MAX_DAYS_PER_WEEK][MAX_HOURS_PER_DAY], QList<qint16> b[TEACHERS_FREE_PERIODS_N_CATEGORIES][MAX_DAYS_PER_WEEK][MAX_HOURS_PER_DAY]){
//void Solution::getTeachersTimetable(Rules& r, Matrix3D<qint16>& a, QList<qint16> b[TEACHERS_FREE_PERIODS_N_CATEGORIES][MAX_DAYS_PER_WEEK][MAX_HOURS_PER_DAY]){
void Solution::getTeachersTimetable(Rules& r, Matrix3D<qint16>& a, Matrix3D<QList<qint16> >& b){
//assert(HFitness()==0); //This is only for perfect solutions, that do not have any non-satisfied hard constrains
assert(r.initialized);
assert(r.internalStructureComputed);
a.resize(r.nInternalTeachers, r.nDaysPerWeek, r.nHoursPerDay);
b.resize(TEACHERS_FREE_PERIODS_N_CATEGORIES, r.nDaysPerWeek, r.nHoursPerDay);
int i, j, k;
for(i=0; i<r.nInternalTeachers; i++)
for(j=0; j<r.nDaysPerWeek; j++)
for(k=0; k<r.nHoursPerDay; k++)
//a1[i][j][k]=a2[i][j][k]=UNALLOCATED_ACTIVITY;
a[i][j][k]=UNALLOCATED_ACTIVITY;
Activity *act;
for(i=0; i<r.nInternalActivities; i++)
if(this->times[i]!=UNALLOCATED_TIME) {
act=&r.internalActivitiesList[i];
int hour=this->times[i]/r.nDaysPerWeek;
int day=this->times[i]%r.nDaysPerWeek;
for(int dd=0; dd < act->duration; dd++){
assert(hour+dd<r.nHoursPerDay);
for(int ti=0; ti<act->iTeachersList.count(); ti++){
int tch = act->iTeachersList.at(ti); //teacher index
/*if(a1[tch][day][hour+dd]==UNALLOCATED_ACTIVITY)
a1[tch][day][hour+dd]=i;
else
a2[tch][day][hour+dd]=i;*/
assert(a[tch][day][hour+dd]==UNALLOCATED_ACTIVITY);
a[tch][day][hour+dd]=i;
}
}
}
//Prepare teachers free periods timetable.
//Code contributed by Volker Dirr (http://timetabling.de/) BEGIN
int d,h,tch;
for(d=0; d<r.nDaysPerWeek; d++){
for(h=0; h<r.nHoursPerDay; h++){
for(int tfp=0; tfp<TEACHERS_FREE_PERIODS_N_CATEGORIES; tfp++){
b[tfp][d][h].clear();
}
}
}
for(tch=0; tch<r.nInternalTeachers; tch++){
for(d=0; d<r.nDaysPerWeek; d++){
int firstPeriod=-1;
int lastPeriod=-1;
for(h=0; h<r.nHoursPerDay; h++){
if(a[tch][d][h]!=UNALLOCATED_ACTIVITY){
if(firstPeriod==-1)
firstPeriod=h;
lastPeriod=h;
}
}
if(firstPeriod==-1){
for(h=0; h<r.nHoursPerDay; h++){
b[TEACHER_HAS_A_FREE_DAY][d][h]<<tch;
}
} else {
for(h=0; h<firstPeriod; h++){
if(firstPeriod-h==1){
b[TEACHER_MUST_COME_EARLIER][d][h]<<tch;
}
else {
b[TEACHER_MUST_COME_MUCH_EARLIER][d][h]<<tch;
}
}
for(; h<lastPeriod+1; h++){
if(a[tch][d][h]==UNALLOCATED_ACTIVITY){
if(a[tch][d][h+1]==UNALLOCATED_ACTIVITY){
if(a[tch][d][h-1]==UNALLOCATED_ACTIVITY){
b[TEACHER_HAS_BIG_GAP][d][h]<<tch;
} else {
b[TEACHER_HAS_BORDER_GAP][d][h]<<tch;
}
} else {
if(a[tch][d][h-1]==UNALLOCATED_ACTIVITY){
b[TEACHER_HAS_BORDER_GAP][d][h]<<tch;
} else {
b[TEACHER_HAS_SINGLE_GAP][d][h]<<tch;
}
}
}
}
for(; h<r.nHoursPerDay; h++){
if(lastPeriod-h==-1){
b[TEACHER_MUST_STAY_LONGER][d][h]<<tch;
}
else {
b[TEACHER_MUST_STAY_MUCH_LONGER][d][h]<<tch;
}
}
}
}
}
//care about not available teacher and breaks
for(tch=0; tch<r.nInternalTeachers; tch++){
for(d=0; d<r.nDaysPerWeek; d++){
for(h=0; h<r.nHoursPerDay; h++){
if(teacherNotAvailableDayHour[tch][d][h]==true || breakDayHour[d][h]==true){
int removed=0;
for(int tfp=0; tfp<TEACHER_IS_NOT_AVAILABLE; tfp++){
if(b[tfp][d][h].contains(tch)){
removed+=b[tfp][d][h].removeAll(tch);
if(breakDayHour[d][h]==false)
b[TEACHER_IS_NOT_AVAILABLE][d][h]<<tch;
}
}
assert(removed==1);
}
}
}
}
//END of Code contributed by Volker Dirr (http://timetabling.de/) END
//bool visited[MAX_TEACHERS];
Matrix1D<bool> visited;
visited.resize(r.nInternalTeachers);
for(d=0; d<r.nDaysPerWeek; d++){
for(h=0; h<r.nHoursPerDay; h++){
for(tch=0; tch<r.nInternalTeachers; tch++)
visited[tch]=false;
for(int tfp=0; tfp<TEACHERS_FREE_PERIODS_N_CATEGORIES; tfp++){
foreach(int tch, b[tfp][d][h]){
assert(!visited[tch]);
visited[tch]=true;
}
}
}
}
bool Interstitial::minimizePoint(Vector3D& point, const ISO& iso, double scale)
{
// Loop until converged
int i;
int minIndex;
int loopNum = 0;
int maxLoops = 50;
int curNegCount;
int origNegCount;
double mag;
double tol = 1e-8;
double stepSize = 1;
double minEigenvalue;
Vector3D step;
Vector3D deriv;
Vector3D eigenvalues;
Vector3D minEigenvector;
Matrix3D eigenvectors;
Matrix3D hessian;
for (; loopNum < maxLoops; ++loopNum)
{
// Get current step, derivatives, and second derivatives
step = getStep(iso, point, deriv, hessian, scale);
// Check if at a stationary point
if (deriv.magnitude() < tol)
{
// Count the number of negative eigenvalues
origNegCount = 0;
eigenvalues = hessian.eigenvalues(&eigenvectors);
for (i = 0; i < 3; ++i)
{
if (eigenvalues[i] < -tol)
++origNegCount;
}
// Found a minimum so break
if (origNegCount == 0)
break;
// Get the most negative eigenvalue
minIndex = 0;
if (eigenvalues[1] < eigenvalues[minIndex])
minIndex = 1;
if (eigenvalues[2] < eigenvalues[minIndex])
minIndex = 2;
minEigenvalue = eigenvalues[minIndex];
minEigenvector[0] = eigenvectors(0, minIndex);
minEigenvector[1] = eigenvectors(1, minIndex);
minEigenvector[2] = eigenvectors(2, minIndex);
// Make step along lowest eigenvector
step = minEigenvector * stepSize;
if (step.magnitude() < tol)
{
loopNum = maxLoops;
break;
}
point -= step;
// Perform gradient based minimization until there is one less negative eigenvalue
for (++loopNum; loopNum < maxLoops; ++loopNum)
{
// Get current derivative
getStep(iso, point, deriv, hessian, scale);
// Count the number of negative eigenvalues
curNegCount = 0;
eigenvalues = hessian.eigenvalues();
for (i = 0; i < 3; ++i)
{
if (eigenvalues[i] < -tol)
++curNegCount;
}
// Break if a negative eigenvalue was removed
if (curNegCount < origNegCount)
break;
// Make step
step = deriv;
step *= stepSize / deriv.magnitude();
point -= step;
}
}
// Make sure that step is not too large and update position
else
{
mag = step.magnitude();
if (mag > 0.25)
step *= 0.25 / mag;
point -= step;
}
}
// Return that point was not found if not converged
if (loopNum >= maxLoops)
return false;
// Return that point was found
return true;
}
static double pmean_of_triangle_corner_hessians( double inner_power,
double outer_power,
const double* v,
const Vector3D* cg,
const Matrix3D* ch,
Vector3D* tg,
Matrix3D* th,
bool scale )
{
Matrix3D op;
double gf[3], hf[3];
double nm, m = 0;
double den = scale ? 3.0: 1.0;
for (unsigned i = 0; i < 3; ++i) {
nm = pow(v[i], inner_power);
m += nm;
gf[i] = inner_power*nm / v[i] / den;
hf[i] = (inner_power-1)*gf[i] / v[i];
}
nm = m / den;
tg[0] = gf[0] * cg[0] + gf[1] * cg[5] + gf[2] * cg[7];
tg[1] = gf[0] * cg[1] + gf[1] * cg[3] + gf[2] * cg[8];
tg[2] = gf[0] * cg[2] + gf[1] * cg[4] + gf[2] * cg[6];
th[0] = hf[0]*op.outer_product( cg[0], cg[0] ) + gf[0]*ch[ 0]
+ hf[1]*op.outer_product( cg[5], cg[5] ) + gf[1]*ch[11]
+ hf[2]*op.outer_product( cg[7], cg[7] ) + gf[2]*ch[15];
th[3] = hf[0]*op.outer_product( cg[1], cg[1] ) + gf[0]*ch[ 3]
+ hf[1]*op.outer_product( cg[3], cg[3] ) + gf[1]*ch[ 6]
+ hf[2]*op.outer_product( cg[8], cg[8] ) + gf[2]*ch[17];
th[5] = hf[0]*op.outer_product( cg[2], cg[2] ) + gf[0]*ch[ 5]
+ hf[1]*op.outer_product( cg[4], cg[4] ) + gf[1]*ch[ 9]
+ hf[2]*op.outer_product( cg[6], cg[6] ) + gf[2]*ch[12];
th[1] = hf[0]*op.outer_product( cg[0], cg[1] ) + gf[0]*ch[ 1]
+ hf[1]*op.outer_product( cg[5], cg[3] ) + gf[1]*transpose(ch[ 8])
+ hf[2]*op.outer_product( cg[7], cg[8] ) + gf[2]*ch[16];
th[2] = hf[0]*op.outer_product( cg[0], cg[2] ) + gf[0]*ch[ 2]
+ hf[1]*op.outer_product( cg[5], cg[4] ) + gf[1]*transpose(ch[10])
+ hf[2]*op.outer_product( cg[7], cg[6] ) + gf[2]*transpose(ch[13]);
th[4] = hf[0]*op.outer_product( cg[1], cg[2] ) + gf[0]*ch[ 4]
+ hf[1]*op.outer_product( cg[3], cg[4] ) + gf[1]*ch[ 7]
+ hf[2]*op.outer_product( cg[8], cg[6] ) + gf[2]*transpose(ch[14]);
m = pow(nm, outer_power);
double g = m * outer_power / nm;
double h = (outer_power - 1.0) * g / nm;
for (unsigned r = 0; r < 3; ++r) {
for (unsigned c = r; c < 3; ++c) {
*th = g * *th + h * op.outer_product( tg[r], tg[c] );
++th;
}
tg[r] *= g;
}
return m;
}
int
main (int argc, char **argv)
{
Context *ctx;
CairoSurface *target;
MoonSurface *surface;
cairo_surface_t *dst, *src;
TransformEffect *effect;
Matrix3D *matrix;
DoubleCollection *values;
int stride = width * 4;
Rect bounds = Rect (0, 0, width, height);
gpointer data;
bool status = true;
int count = 1;
if (argc < 2) {
printf ("usage: %s MATRIX [COUNT]\n", argv[0]);
return 1;
}
gtk_init (&argc, &argv);
runtime_init_desktop ();
values = DoubleCollection::FromStr (argv[1]);
if (!values) {
printf ("usage: %s MATRIX [COUNT]\n", argv[0]);
return 1;
}
if (values->GetCount () != 16) {
printf ("usage: %s MATRIX [COUNT]\n", argv[0]);
values->unref ();
return 1;
}
data = g_malloc0 (height * stride);
dst = cairo_image_surface_create_for_data ((unsigned char *) data,
CAIRO_FORMAT_ARGB32,
width, height, stride);
target = new CairoSurface (dst);
cairo_surface_destroy (dst);
ctx = new CairoContext (target);
target->unref ();
src = cairo_surface_create_similar (dst,
CAIRO_CONTENT_COLOR_ALPHA,
width, height);
surface = new CairoSurface (src);
cairo_surface_destroy (src);
effect = new TransformEffect ();
matrix = new Matrix3D ();
matrix->SetM11 (values->GetValueAt (0)->AsDouble ());
matrix->SetM12 (values->GetValueAt (1)->AsDouble ());
matrix->SetM13 (values->GetValueAt (2)->AsDouble ());
matrix->SetM14 (values->GetValueAt (3)->AsDouble ());
matrix->SetM21 (values->GetValueAt (4)->AsDouble ());
matrix->SetM22 (values->GetValueAt (5)->AsDouble ());
matrix->SetM23 (values->GetValueAt (6)->AsDouble ());
matrix->SetM24 (values->GetValueAt (7)->AsDouble ());
matrix->SetM31 (values->GetValueAt (8)->AsDouble ());
matrix->SetM32 (values->GetValueAt (9)->AsDouble ());
matrix->SetM33 (values->GetValueAt (10)->AsDouble ());
matrix->SetM34 (values->GetValueAt (11)->AsDouble ());
matrix->SetOffsetX (values->GetValueAt (12)->AsDouble ());
matrix->SetOffsetY (values->GetValueAt (13)->AsDouble ());
matrix->SetOffsetZ (values->GetValueAt (14)->AsDouble ());
matrix->SetM44 (values->GetValueAt (15)->AsDouble ());
values->unref ();
if (argc > 2) {
count = atoi (argv[2]);
if (count > 1) {
signal (SIGALRM, projection_alarm_handler);
alarm (5);
}
}
while (status && count-- > 0) {
status = effect->Render (ctx,
surface,
(double *) matrix->GetMatrixValues (),
0, 0, width, height);
frames++;
}
matrix->unref ();
surface->unref ();
delete ctx;
g_free (data);
runtime_shutdown ();
return status != true;
}
Matrix3D Matrix3D::inverse() const {
// TODO: refactor to consider false cases
Matrix3D out;
out.matrix[0] = matrix[5] * matrix[10] * matrix[15] -
matrix[5] * matrix[11] * matrix[14] -
matrix[9] * matrix[6] * matrix[15] +
matrix[9] * matrix[7] * matrix[14] +
matrix[13] * matrix[6] * matrix[11] -
matrix[13] * matrix[7] * matrix[10];
out.matrix[4] = -matrix[4] * matrix[10] * matrix[15] +
matrix[4] * matrix[11] * matrix[14] +
matrix[8] * matrix[6] * matrix[15] -
matrix[8] * matrix[7] * matrix[14] -
matrix[12] * matrix[6] * matrix[11] +
matrix[12] * matrix[7] * matrix[10];
out.matrix[8] = matrix[4] * matrix[9] * matrix[15] -
matrix[4] * matrix[11] * matrix[13] -
matrix[8] * matrix[5] * matrix[15] +
matrix[8] * matrix[7] * matrix[13] +
matrix[12] * matrix[5] * matrix[11] -
matrix[12] * matrix[7] * matrix[9];
out.matrix[12] = -matrix[4] * matrix[9] * matrix[14] +
matrix[4] * matrix[10] * matrix[13] +
matrix[8] * matrix[5] * matrix[14] -
matrix[8] * matrix[6] * matrix[13] -
matrix[12] * matrix[5] * matrix[10] +
matrix[12] * matrix[6] * matrix[9];
out.matrix[1] = -matrix[1] * matrix[10] * matrix[15] +
matrix[1] * matrix[11] * matrix[14] +
matrix[9] * matrix[2] * matrix[15] -
matrix[9] * matrix[3] * matrix[14] -
matrix[13] * matrix[2] * matrix[11] +
matrix[13] * matrix[3] * matrix[10];
out.matrix[5] = matrix[0] * matrix[10] * matrix[15] -
matrix[0] * matrix[11] * matrix[14] -
matrix[8] * matrix[2] * matrix[15] +
matrix[8] * matrix[3] * matrix[14] +
matrix[12] * matrix[2] * matrix[11] -
matrix[12] * matrix[3] * matrix[10];
out.matrix[9] = -matrix[0] * matrix[9] * matrix[15] +
matrix[0] * matrix[11] * matrix[13] +
matrix[8] * matrix[1] * matrix[15] -
matrix[8] * matrix[3] * matrix[13] -
matrix[12] * matrix[1] * matrix[11] +
matrix[12] * matrix[3] * matrix[9];
out.matrix[13] = matrix[0] * matrix[9] * matrix[14] -
matrix[0] * matrix[10] * matrix[13] -
matrix[8] * matrix[1] * matrix[14] +
matrix[8] * matrix[2] * matrix[13] +
matrix[12] * matrix[1] * matrix[10] -
matrix[12] * matrix[2] * matrix[9];
out.matrix[2] = matrix[1] * matrix[6] * matrix[15] -
matrix[1] * matrix[7] * matrix[14] -
matrix[5] * matrix[2] * matrix[15] +
matrix[5] * matrix[3] * matrix[14] +
matrix[13] * matrix[2] * matrix[7] -
matrix[13] * matrix[3] * matrix[6];
out.matrix[6] = -matrix[0] * matrix[6] * matrix[15] +
matrix[0] * matrix[7] * matrix[14] +
matrix[4] * matrix[2] * matrix[15] -
matrix[4] * matrix[3] * matrix[14] -
matrix[12] * matrix[2] * matrix[7] +
matrix[12] * matrix[3] * matrix[6];
out.matrix[10] = matrix[0] * matrix[5] * matrix[15] -
matrix[0] * matrix[7] * matrix[13] -
matrix[4] * matrix[1] * matrix[15] +
matrix[4] * matrix[3] * matrix[13] +
matrix[12] * matrix[1] * matrix[7] -
matrix[12] * matrix[3] * matrix[5];
out.matrix[14] = -matrix[0] * matrix[5] * matrix[14] +
matrix[0] * matrix[6] * matrix[13] +
matrix[4] * matrix[1] * matrix[14] -
matrix[4] * matrix[2] * matrix[13] -
matrix[12] * matrix[1] * matrix[6] +
matrix[12] * matrix[2] * matrix[5];
out.matrix[3] = -matrix[1] * matrix[6] * matrix[11] +
matrix[1] * matrix[7] * matrix[10] +
matrix[5] * matrix[2] * matrix[11] -
matrix[5] * matrix[3] * matrix[10] -
matrix[9] * matrix[2] * matrix[7] +
matrix[9] * matrix[3] * matrix[6];
out.matrix[7] = matrix[0] * matrix[6] * matrix[11] -
matrix[0] * matrix[7] * matrix[10] -
matrix[4] * matrix[2] * matrix[11] +
matrix[4] * matrix[3] * matrix[10] +
matrix[8] * matrix[2] * matrix[7] -
matrix[8] * matrix[3] * matrix[6];
out.matrix[11] = -matrix[0] * matrix[5] * matrix[11] +
matrix[0] * matrix[7] * matrix[9] +
matrix[4] * matrix[1] * matrix[11] -
matrix[4] * matrix[3] * matrix[9] -
matrix[8] * matrix[1] * matrix[7] +
matrix[8] * matrix[3] * matrix[5];
out.matrix[15] = matrix[0] * matrix[5] * matrix[10] -
matrix[0] * matrix[6] * matrix[9] -
matrix[4] * matrix[1] * matrix[10] +
matrix[4] * matrix[2] * matrix[9] +
matrix[8] * matrix[1] * matrix[6] -
matrix[8] * matrix[2] * matrix[5];
float det;
det = matrix[0] * out.matrix[0] + matrix[1] * out.matrix[4] + matrix[2] * out.matrix[8] + matrix[3] * out.matrix[12];
if (det == 0)
out.init();
return out;
det = 1.0f / det;
for (int i = 0; i < 16; i++)
out.matrix[i] = out.matrix[i] * det;
return out;
}
void Matrix3D::_RotateAroundWorldRightVector(double fAmt) {
Matrix3D rotate = _CreateRotationAroundWorldRightVector(fAmt);
Matrix3D result = rotate._Multiply(*this);
this->_SetEqualTo(result);
}
void Matrix3D::_TranslateAlongWorldRight(double fAmt) {
Matrix3D translate = _CreateTranslateAlongWorldRight(fAmt);
Matrix3D result = translate._Multiply(*this);
this->_SetEqualTo(result);
}
Vector3D Interstitial::getStep(const ISO& iso, const Vector3D& point, Vector3D& deriv, Matrix3D& H, double scale)
{
// Clear data
deriv = 0.0;
H = 0.0;
// Get fractional coordinates of current point
Vector3D fracPoint = iso.basis().getFractional(point);
ISO::moveIntoCell(fracPoint);
// Loop over atoms
int i, j, k;
double dis;
double norm;
double denom;
double cutoff;
double exponent;
Vector3D dif;
ImageIterator images;
for (i = 0; i < iso.atoms().length(); ++i)
{
// Set cutoff for current element
norm = scale * iso.atoms()[i][0].element().radius();
cutoff = -log(1e-8) * norm;
// Set image iterator
images.setCell(iso.basis(), cutoff);
// Loop over atoms of current element
for (j = 0; j < iso.atoms()[i].length(); ++j)
{
// Reset image iterator for current atom
images.reset(fracPoint, iso.atoms()[i][j].fractional());
// Loop over images
while (!images.finished())
{
// Get current value
dis = ++images;
if (dis < 1e-8)
continue;
exponent = exp(-dis / norm);
// Get derivative vector
dif = images.cartVector();
dif *= -1;
for (k = 0; k < 3; ++k)
deriv[k] += -exponent * dif[k] / (norm * dis);
// Get second derivative vector
denom = norm * norm * pow(dis, 3);
H(0, 0) += exponent * (dif[0]*dif[0]*dis - norm * (dif[1]*dif[1] + dif[2]*dif[2])) / denom;
H(1, 1) += exponent * (dif[1]*dif[1]*dis - norm * (dif[0]*dif[0] + dif[2]*dif[2])) / denom;
H(2, 2) += exponent * (dif[2]*dif[2]*dis - norm * (dif[0]*dif[0] + dif[1]*dif[1])) / denom;
// Add to Hessian
H(0, 1) += exponent * dif[0]*dif[1] * (norm + dis) / denom;
H(0, 2) += exponent * dif[0]*dif[2] * (norm + dis) / denom;
H(1, 2) += exponent * dif[1]*dif[2] * (norm + dis) / denom;
}
}
}
// Finish building Hessian
H(1, 0) = H(0, 1);
H(2, 0) = H(0, 2);
H(2, 1) = H(1, 2);
// Return Newton step
return H.inverse() * deriv;
}
