//-----------------------------------------------------------------------------
 Matrix2& invert(T_Scalar *determinant=NULL)
 {
   T_Scalar det = getInverse(*this);
   if (determinant)
     *determinant = det;
   return *this;
 }
Esempio n. 2
0
 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;
      }
    }
Esempio n. 4
0
			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;
			}
Esempio n. 5
0
 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);
 }
Esempio n. 6
0
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};
}
Esempio n. 7
0
 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;
 }
Esempio n. 9
0
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;
}
Esempio n. 10
0
	/// 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;
	}
Esempio n. 11
0
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;
}
Esempio n. 12
0
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);
        }
Esempio n. 13
0
void C7Vector::inverse()
{
	(*this)=getInverse();
}
Esempio n. 14
0
			//----------
			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);
			}
Esempio n. 15
0
 Matrix4& Matrix4::inverse() {
   return (*this = getInverse());
 }
Esempio n. 16
0
 Matrix3& Matrix3::inverse() {
   return (*this = getInverse());
 }
Esempio n. 17
0
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();
}
Esempio n. 18
0
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 ) ;
}
Esempio n. 19
0
	/**
	 * return the inverse transpose of the matrix
	 */
	CMatrix2<T> getInverseTranspose() const
	{
		// TODO: optimize me
		CMatrix2<T> m = getInverse();
		return m.getTranspose();
	}
Esempio n. 20
0
void
CMatrix::invert(
	)
{
	*this = getInverse();
}
Esempio n. 21
0
void Matrixf44::MakeInverse()
{
    *this = getInverse();
}