//-----------------------------------------------------------------------------
Matrix2& invert(T_Scalar *determinant=NULL)
{
T_Scalar det = getInverse(*this);
if (determinant)
*determinant = det;
return *this;
}
Vector3 Transform::invertPoint(const Vector3& p) const {
const Matrix4& M = getInverse();
return Vector3(
M[0][0] * p.x + M[0][1] * p.y + M[0][2] * p.z + M[0][3],
M[1][0] * p.x + M[1][1] * p.y + M[1][2] * p.z + M[1][3],
M[2][0] * p.x + M[2][1] * p.y + M[2][2] * p.z + M[2][3]);
}
//-----------------------------------------------------------------------------
T_Scalar getInverse(Matrix2& dest) const
{
if (&dest == this)
{
Matrix2 tmp;
T_Scalar det = getInverse(tmp);
dest = tmp;
return det;
}
else
{
const T_Scalar& a11 = e(0,0);
const T_Scalar& a12 = e(1,0);
const T_Scalar& a21 = e(0,1);
const T_Scalar& a22 = e(1,1);
dest.fill(0);
T_Scalar det = a11*a22-a12*a21;
if (det != 0)
dest = Matrix2(+a22, -a12, -a21, +a11) / det;
return det;
}
}
CheckOverlapResult invertIncludingOrientation() const
{
CheckOverlapResult o = *this;
if (
isLeftEdge(o.orientation)
||
isRightEdge(o.orientation)
)
std::swap(o.clipa,o.clipb);
std::swap(o.numins,o.numdel);
std::swap(o.ia,o.ib);
o.orientation = getInverse(o.orientation);
std::reverse(o.trace.begin(),o.trace.end());
for ( uint64_t i = 0; i < o.trace.size(); ++i )
switch ( o.trace[i] )
{
case 'I': o.trace[i] = 'D'; break;
case 'D': o.trace[i] = 'I'; break;
default: break;
}
return o;
}
Vector3 Transform::onNormal(const Vector3& n) const {
const Matrix4& invT = getInverse().transpose();
return Vector3(
invT[0][0] * n.x + invT[0][1] * n.y + invT[0][2] * n.z,
invT[1][0] * n.x + invT[1][1] * n.y + invT[1][2] * n.z,
invT[2][0] * n.x + invT[2][1] * n.y + invT[2][2] * n.z);
}
Quaternion Quaternion::slerp(const Quaternion& q, float t) const
{
auto q0 = Quaternion();
auto cosTheta = x * q.x + y * q.y + z * q.z + w * q.w;
if (cosTheta < 0.0f)
{
q0 = getInverse();
cosTheta = -cosTheta;
}
else
q0 = *this;
auto a = 0.0f;
auto b = 0.0f;
if (1.0f - cosTheta > Math::Epsilon)
{
// Use spherical interpolation
auto theta = acosf(cosTheta);
auto oosinTheta = 1.0f / sinf(theta);
a = sinf(theta * t) * oosinTheta;
b = sinf(theta * (1.0f - t)) * oosinTheta;
}
else
{
// Use linear interpolation
a = t;
b = 1.0f - t;
}
// Do the interpolation
return {q0.x * b + q.x * a, q0.y * b + q.y * a, q0.z * b + q.z * a, q0.w * b + q.w * a};
}
Vector3 Transform::invertVector(const Vector3& v) const {
const Matrix4& M = getInverse();
return Vector3(
M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z,
M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z,
M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z);
}
//-----------------------------------------------------------------------------
Matrix2 getInverse(T_Scalar *determinant=NULL) const
{
Matrix2 tmp;
T_Scalar det = getInverse(tmp);
if (determinant)
*determinant = det;
return tmp;
}
bool Box::intersect( Ray const& r, HitProperties* hp ) const {
// ray in object space
Ray ray( getInverse() * r.origin, ( getInverse() * r.direction ).normalized() );
double tnear, tfar;
bool inside;
Vector nrm;
if( Intersect::RayBox( ray, Vector( 0.0, 0.0, 0.0 ), min, max, &tnear, &tfar, &inside, &nrm ) ) {
hp->distance = tnear;
hp->normal = nrm; // os normal
hp->ray = r;
hp->inside = inside;
hp->objectPtr = const_cast<Box*>( this );
return true;
}
return false;
}
/// solve for rotational Acceleration with No Constraints
Vect3 BodyRigid::solveRotAccel(bool storeAccels)
{
/// page 292 Nikravesh equation 11.18
Mat3x3 wskew = skew(m_wl);
/// TODO, see if line below is faster, or more accurate
//m_wldot = m_inertia.colPivHouseholderQr().solve(m_appliedTorque-wskew*m_inertia*m_wl);
//m_wldot = m_inertia.ldlt().solve(m_appliedTorque-wskew*m_inertia*m_wl);
Mat3x3 Iinv = getInverse(m_inertia);
Vect3 wldot = Iinv * (m_appliedTorque-wskew*m_inertia*m_wl);
if (storeAccels)
{
m_wldot = wldot;
}
return wldot;
}
std::pair<Matrix, Matrix> Instance::makeMatrices(const double time)
{
auto it = memo.find(time);
if (it != memo.end())
return it->second;
static const double degreesToRadians = M_PI/180.0;
auto tt = t(time);
auto st = s(time);
auto m = translate(tt.x,tt.y,tt.z);
m *= rotateX(rx(time)*degreesToRadians);
m *= rotateY(ry(time)*degreesToRadians);
m *= rotateZ(rz(time)*degreesToRadians);
m *= scale(st.x,st.y,st.z);
auto n = m.getInverse();
auto entry = std::make_pair(time, std::make_pair(m, n));
memo.insert(entry);
return entry.second;
}
void CseSpotter::analyseExpr(IHqlExpression * expr)
{
CseSpotterInfo * extra = queryBodyExtra(expr);
if (!extra->annotatedExpr && expr->isAnnotation())
extra->annotatedExpr = expr;
if (isAssociated)
extra->numAssociatedRefs++;
node_operator op = expr->getOperator();
#ifdef OPTIMIZE_INVERSE
if (getInverseOp(op) != no_none)
{
OwnedHqlExpr inverse = getInverse(expr);
CseSpotterInfo * inverseExtra = queryBodyExtra(inverse);
extra->inverse = inverseExtra;
inverseExtra->inverse = extra;
}
#endif
if (op == no_alias)
{
queryBodyExtra(expr->queryChild(0))->alreadyAliased = true;
extra->alreadyAliased = true;
}
switch (op)
{
case no_assign:
case no_transform:
case no_newtransform:
case no_range:
case no_rangefrom:
if (expr->isConstant())
return;
break;
case no_constant:
return;
}
if (extra->numRefs++ != 0)
{
if (op == no_alias)
return;
if (!spottedCandidate && extra->worthAliasing())
spottedCandidate = true;
if (canCreateTemporary(expr))
return;
//Ugly! This is here as a temporary hack to stop branches of maps being commoned up and always
//evaluated. The alias spotting and generation really needs to take conditionality into account....
if (op == no_mapto)
return;
}
if (!containsPotentialCSE(expr))
return;
if (canAlias && !expr->isDataset())
extra->canAlias = true;
bool savedCanAlias = canAlias;
if (expr->isDataset() && (op != no_select) && (!spotCseInIfDatasetConditions || (op != no_if)))
{
//There is little point looking for CSEs within dataset expressions, because only a very small
//minority which would correctly cse, and it can cause lots of problems - e.g., join conditions.
unsigned first = getFirstActivityArgument(expr);
unsigned num = getNumActivityArguments(expr);
HqlExprArray children;
bool defaultCanAlias = canAlias;
ForEachChild(i, expr)
{
IHqlExpression * cur = expr->queryChild(i);
if (i >= first && i < first+num)
canAlias = defaultCanAlias;
else
canAlias = false;
analyseExpr(cur);
}
void C7Vector::inverse()
{
(*this)=getInverse();
}
//----------
void UpdateTracking::processFrame(shared_ptr<MatchMarkersFrame> incomingFrame) {
//ignore if less than 3
if (!(incomingFrame->result.success || incomingFrame->result.forceTakeTransform)) {
//do nothing
return;
}
//construct output
auto outgoingFrame = make_shared<UpdateTrackingFrame>();
outgoingFrame->incomingFrame = incomingFrame;
outgoingFrame->updateTarget = this->parameters.updateTarget;
outgoingFrame->bodyModelViewRotationVector = incomingFrame->modelViewRotationVector;
outgoingFrame->bodyModelViewTranslation = incomingFrame->modelViewTranslation;
if (incomingFrame->result.forceTakeTransform) {
outgoingFrame->bodyModelViewRotationVector = incomingFrame->modelViewRotationVector;
outgoingFrame->bodyModelViewTranslation = incomingFrame->modelViewTranslation;
}
else {
cv::solvePnP(incomingFrame->result.objectSpacePoints
, incomingFrame->result.centroids
, incomingFrame->cameraDescription->cameraMatrix
, incomingFrame->cameraDescription->distortionCoefficients
, outgoingFrame->bodyModelViewRotationVector
, outgoingFrame->bodyModelViewTranslation
, true);
}
//ignore if reprojection error is too high
{
cv::projectPoints(incomingFrame->result.objectSpacePoints
, outgoingFrame->bodyModelViewRotationVector
, outgoingFrame->bodyModelViewTranslation
, incomingFrame->cameraDescription->cameraMatrix
, incomingFrame->cameraDescription->distortionCoefficients
, outgoingFrame->reprojectedAfterTracking);
float sumErrorSquared = 0.0f;
for (int i = 0; i < incomingFrame->result.count; i++) {
const auto delta = outgoingFrame->reprojectedAfterTracking[i] - incomingFrame->result.centroids[i];
sumErrorSquared += delta.dot(delta);
}
auto reprojectionError = sqrt(sumErrorSquared);
this->reprojectionError.store(reprojectionError);
auto reprojectionThreshold = this->parameters.reprojectionThreshold.get();
if (reprojectionError > reprojectionThreshold && !incomingFrame->result.forceTakeTransform) {
//reprojection error is too high
return;
}
}
//apply the inverse of the camera transform
{
auto cameraTransform = ofxCv::makeMatrix(incomingFrame->cameraDescription->inverseRotationVector, incomingFrame->cameraDescription->inverseTranslation);
cv::Mat cameraRotationInverse;
cv::Mat cameraTranslationInverse;
ofxCv::decomposeMatrix(cameraTransform.getInverse()
, cameraRotationInverse
, cameraTranslationInverse);
cv::composeRT(outgoingFrame->bodyModelViewRotationVector
, outgoingFrame->bodyModelViewTranslation
, cameraRotationInverse
, cameraTranslationInverse
, outgoingFrame->modelRotationVector
, outgoingFrame->modelTranslation);
}
switch (outgoingFrame->updateTarget.get()) {
case UpdateTarget::Body:
{
//nothing to do
outgoingFrame->transform = ofxCv::makeMatrix(outgoingFrame->modelRotationVector
, outgoingFrame->modelTranslation);
break;
}
case UpdateTarget::Camera:
{
//special case, only the transform is in camera coords
outgoingFrame->transform = ofxCv::makeMatrix(outgoingFrame->bodyModelViewRotationVector
, outgoingFrame->bodyModelViewTranslation).getInverse() * incomingFrame->bodyDescription->modelTransform;
break;
}
default:
break;
}
this->onNewFrame.notifyListeners(outgoingFrame);
this->trackingUpdateToMainThread.send(outgoingFrame);
}
Matrix4& Matrix4::inverse() {
return (*this = getInverse());
}
Matrix3& Matrix3::inverse() {
return (*this = getInverse());
}
void Box::completeHitProperties( HitProperties* hp ) const {
// should work..
// hp->point = hp->ray.origin + hp->distance * hp->ray.direction;
Vector osPos = getInverse() * hp->ray.origin;
Vector osDir = ( getInverse() * hp->ray.direction ).normalized();
// calc os hitpoint
hp->point = osPos + hp->distance * osDir;
// calc extend lengths
Vector len( max - min );
Vector pos( hp->point );
// find os binormal & tangent
// find tex coords depending on os normal + os hitpoint
// bias/scale into [0,1]
if( hp->normal.x == -1.0 ) {
hp->texel.x = ( -min.z + pos.z ) / len.z;
hp->texel.y = 1.0 - ( -min.y + pos.y ) / len.y;
if( material->hasNormalMap() ) {
hp->tangent = Vector( 0.0, 0.0, -1.0 );
hp->binormal= Vector( 0.0, 1.0, 0.0 );
}
}
if( hp->normal.x == 1.0 ) {
hp->texel.x = 1.0 - ( -min.z + pos.z ) / len.z;
hp->texel.y = 1.0 - ( -min.y + pos.y ) / len.y;
if( material->hasNormalMap() ) {
hp->tangent = Vector( 0.0, 0.0, 1.0 );
hp->binormal= Vector( 0.0, -1.0, 0.0 );
}
}
///
if( hp->normal.y == -1.0 ) {
hp->texel.x = ( -min.x + pos.x ) / len.x;
hp->texel.y = 1.0 - ( -min.z + pos.z ) / len.z;
if( material->hasNormalMap() ) {
hp->tangent = Vector( 0.0, 0.0, 1.0 );
hp->binormal= Vector( -1.0, 0.0, 0.0 );
}
}
if( hp->normal.y == 1.0 ) {
hp->texel.x = ( -min.x + pos.x ) / len.x;
hp->texel.y = ( -min.z + pos.z ) / len.z;
if( material->hasNormalMap() ) {
hp->tangent = Vector( 0.0, 0.0, 1.0 );
hp->binormal= Vector( 1.0, 0.0, 0.0 );
}
}
///
if( hp->normal.z == -1.0 ) {
hp->texel.x = 1.0 - ( -min.x + pos.x ) / len.x;
hp->texel.y = 1.0 - ( -min.y + pos.y ) / len.y;
if( material->hasNormalMap() ) {
hp->tangent = Vector( 0.0, 1.0, 1.0 );
hp->binormal= Vector( 1.0, 0.0, 0.0 );
}
}
if( hp->normal.z == 1.0 ) {
hp->texel.x = ( -min.x + pos.x ) / len.x;
hp->texel.y = 1.0 - ( -min.y + pos.y ) / len.y;
if( material->hasNormalMap() ) {
hp->tangent = Vector( 0.0, -1.0, 1.0 );
hp->binormal= Vector( 1.0, 0.0, 0.0 );
}
}
// into ws
hp->point = getMatrix() * hp->point;
// tile
hp->texel.x *= getTiling().x;
hp->texel.y *= getTiling().y;
// transform normal to ws
hp->normal.w = 0.0;
hp->normal = getInverse().transposed() * hp->normal;
hp->normal.w = 0.0;
hp->normal.normalize();
// transform tangent to ws
hp->tangent.w = 0.0;
hp->tangent = getInverse().transposed() * hp->tangent;
hp->tangent.w = 0.0;
hp->tangent.normalize();
// transform binormal to ws
hp->binormal.w = 0.0;
hp->binormal = getInverse().transposed() * hp->binormal;
hp->binormal.w = 0.0;
hp->binormal.normalize();
}
bool Box::intersect( Ray const& r, double dist ) const {
// ray in object space
Ray ray( getInverse() * r.origin, ( getInverse() * r.direction ).normalized() );
return Intersect::RayBox( ray, Vector( 0.0, 0.0, 0.0 ), min, max, dist ) ;
}
/**
* return the inverse transpose of the matrix
*/
CMatrix2<T> getInverseTranspose() const
{
// TODO: optimize me
CMatrix2<T> m = getInverse();
return m.getTranspose();
}
void
CMatrix::invert(
)
{
*this = getInverse();
}
void Matrixf44::MakeInverse()
{
*this = getInverse();
}