void GateCombinerImpl::combine(GatePtr pGate1, GatePtr pGate2, GatePtr& result) {
result = NullPtr;
for(unsigned int i = 0; i< m_combinabilityCheckers.size(); i++) {
if(!m_combinabilityCheckers[i]->canCombine(pGate1, pGate2)) {
return;
}
}
MatrixPtr pCombinedMatrix = NullPtr;
m_pMatrixOperator->multiply(pGate1->getMatrix(), pGate2->getMatrix(), pCombinedMatrix);
//If combined matrix is actually identity, abort the combination
if(!checkIdentity(pCombinedMatrix)) {
_destroy(pCombinedMatrix);
return;
}
cost_t combinedCost = pGate1->getCost() + pGate2->getCost();
LabelSeq combinedLabel;
combinedLabel.insert(combinedLabel.end(), pGate1->getLabelSeq().begin(), pGate1->getLabelSeq().end());
combinedLabel.insert(combinedLabel.end(), pGate2->getLabelSeq().begin(), pGate2->getLabelSeq().end());
std::string combinedLabelStr = pGate1->getLabelStr() + pGate2->getLabelStr();
result = GatePtr(new Gate(pCombinedMatrix, combinedCost, combinedLabel, combinedLabelStr));
}
// TEST 6 - WORLD-TO-EXTRINSICS AND EXTRINSICS-TO-WORLD TRANSFORMATIONS
bool test6(vcg::Shotd shot1, vcg::Shotd shot2, vcg::Point3d p1, vcg::Point3d p2)
{
vcg::Matrix44d WtoE1 = shot1.GetWorldToExtrinsicsMatrix();
vcg::Matrix44d WtoE2 = shot2.GetWorldToExtrinsicsMatrix();
vcg::Matrix44d EtoW1 = shot1.GetExtrinsicsToWorldMatrix();
vcg::Matrix44d EtoW2 = shot2.GetExtrinsicsToWorldMatrix();
vcg::Matrix44d I1 = WtoE1 * EtoW1;
vcg::Matrix44d I2 = WtoE2 * EtoW2;
vcg::Matrix44d I3 = EtoW1 * WtoE1;
vcg::Matrix44d I4 = EtoW2 * WtoE2;
if (checkIdentity(I1) > precision)
return false;
if (checkIdentity(I2) > precision)
return false;
if (checkIdentity(I3) > precision)
return false;
if (checkIdentity(I4) > precision)
return false;
vcg::Point3d axisX(1.0, 0.0, 0.0);
vcg::Point3d axisY(0.0, 1.0, 0.0);
vcg::Point3d axisZ(0.0, 0.0, 1.0);
vcg::Point3d vx = EtoW1 * axisX;
vcg::Point3d vy = EtoW1 * axisY;
vcg::Point3d vz = EtoW1 * axisZ;
if (dist3(vx, shot1.Extrinsics.Tra() + shot1.Extrinsics.Rot().GetRow3(0)) > precision)
return false;
if (dist3(vy, shot1.Extrinsics.Tra() + shot1.Extrinsics.Rot().GetRow3(1)) > precision)
return false;
if (dist3(vz, shot1.Extrinsics.Tra() + shot1.Extrinsics.Rot().GetRow3(2)) > precision)
return false;
return true;
}
