C++ Matrix3::Invert方法代码示例

//This will set the formation
void FormationBhvr::SetFormation(TimeValue t)
{
	INode *node;
	Matrix3 tempMatrix;


	//Make sure that the leader is not part of the follower array..
	RemoveLeaderFromFormation(t);


	INode *leader =  GetLeader(t);

	if(leader==NULL)
		return;
	Matrix3 leaderPosition = GetCurrentMatrix(leader,t);
	leaderPosition.NoScale(); //kill any scale if we have it 
	leaderPosition.Invert();  //it's inverted...


	int numDelegates = GetFollowerCount(t);	

	//zero out the formation matrix that's used for saving it out.
	pblock->ZeroCount(follower_matrix1);
	pblock->ZeroCount(follower_matrix2);
	pblock->ZeroCount(follower_matrix3);
	pblock->ZeroCount(follower_matrix4);

	for(int i =0;i<numDelegates;i++)
	{

		node = GetFollower(t,i);
		if(node)
		{
			tempMatrix = GetCurrentMatrix(node,t);
			tempMatrix.NoScale();
			Matrix3 leaderMat =tempMatrix*leaderPosition;
			AppendFollowerMatrix(t,leaderMat);

			//killed because matrix3 wasn't working ...pblock->Append(follower_matrix,1,&leaderMat);
		 
		
		}
		else
		{
			//we still set up follower_matrix so that the counts
			//of the follower_matrix tab and the follower tab are equal.
			tempMatrix.IdentityMatrix();
			AppendFollowerMatrix(t,tempMatrix);

			//pblock->Append(follower_matrix,1,&tempMat);
	
		}
	}

}

Matrix3 Exporter::getObjectTransform(INode *node, TimeValue t, bool local)
{
   Matrix3 tm = node->GetObjTMAfterWSM(t);
   if (local)
   {
      INode *parent = node->GetParentNode();
      if (parent != NULL) {
         Matrix3 pm = parent->GetNodeTM(t);
         pm.Invert();
         tm *= pm;
      }
   }
   return tm;
}

bool CExporter::generateSkinBone(grp::SkinnedMeshExporter* skinnedMesh, INode* node, ISkin* skin)
{
	Matrix3 mSkinOffset;
	if (!skin->GetSkinInitTM(node, mSkinOffset, true))
	{
		setLastError("获取Skin偏移矩阵失败");
		return false;
	}

	int iNumBone = skin->GetNumBones();
	skinnedMesh->m_boneNames.resize(iNumBone);
	skinnedMesh->m_offsetMatrices.resize(iNumBone);
	for (int i = 0; i < iNumBone; ++i)
	{
		//mbs only
		INode* pBone = skin->GetBone(i);
		if (NULL == pBone)
		{
			setLastError("找不到skin依赖的骨骼");
			return false;
		}
		if (ENUM_NODE_DUMMY == checkNodeType(pBone))
		{
			char szError[1024];
			::StringCchPrintf(szError, sizeof(szError), "发现绑定到虚拟体[%s]的顶点", pBone->GetName());
			setLastError(szError);
			return false;
		}

		wchar_t unicodeString[256];
		mbstowcs(unicodeString, pBone->GetName(), 255);
		skinnedMesh->m_boneNames[i] = unicodeString;

		Matrix3 mBoneOffset;
		skin->GetBoneInitTM(pBone, mBoneOffset);
		mBoneOffset.Invert();
		Matrix3 meshOffset = node->GetObjectTM(0);
		mBoneOffset = meshOffset * mBoneOffset;
		//mBoneOffset = mSkinOffset * mBoneOffset;
		grp::Matrix& mCore = skinnedMesh->m_offsetMatrices[i];
		::MatrixFromMatrix3(mCore, mBoneOffset);
	}
	return true;
}

Matrix3 CActionExporter::get_bone_tm(CSkeletonExporter* pSkeleton,int boneIndex,unsigned int iMaxTime)
{
         sMaxBoneNode_t bone = pSkeleton->m_MaxBones[boneIndex];
         Matrix3        boneInitMTInv;
         if(G_MaxEnv().m_bUseBeforeSkeletonPose)
             boneInitMTInv = bone.m_SkinInitMT;
         else
             boneInitMTInv = bone.m_InitNodeTM0;

         boneInitMTInv.Invert();
         INode* pNode = bone.m_pNode;

         Matrix3 BoneTM = boneInitMTInv * pNode->GetNodeTM(iMaxTime); //>GetNodeTM(iMaxTime);
         Point3 Trans = BoneTM.GetTrans();
         BoneTM.NoTrans();
         //对骨骼进行缩放
         Trans.x *= pSkeleton->m_fScale;
         Trans.y *= pSkeleton->m_fScale;
         Trans.z *= pSkeleton->m_fScale;
         BoneTM.SetTrans(Trans);

         return BoneTM;

}

//=============================================================================================
NL3D::IShape *CExportNel::buildRemanence(INode& node, TimeValue time)
{
	std::auto_ptr<CSegRemanenceShape> srs(new CSegRemanenceShape);
	uint  numSlices = getScriptAppData (&node, NEL3D_APPDATA_REMANENCE_SLICE_NUMBER, 2);
	float samplingPeriod = getScriptAppData (&node, NEL3D_APPDATA_REMANENCE_SAMPLING_PERIOD, 0.02f);
	float rollupRatio    = getScriptAppData (&node, NEL3D_APPDATA_REMANENCE_ROLLUP_RATIO, 1.f);

	if (samplingPeriod <= 0.f) samplingPeriod = 0.02f;
	if (numSlices <= 2) numSlices = 2;
	if (rollupRatio <= 0) rollupRatio = 1.f;
	
	srs->setNumSlices((uint32) numSlices);
	srs->setSliceTime(samplingPeriod);
	srs->setRollupRatio(rollupRatio);

	// get material from this node
	std::vector<NL3D::CMaterial> materials;
	CMaxMeshBaseBuild mmbb;
	buildMaterials(materials, mmbb, node, time);
	if (materials.size() != 1)
	{	
		buildRemanenceError(this, node, "The remanent segment %s should have a single material");
		return NULL;
	}
	srs->setMaterial(materials[0]);
	// 
	// get geometry

	ObjectState os = node.EvalWorldState(_Ip->GetTime());		
	Object *obj = node.EvalWorldState(time).obj;	
	if (obj->SuperClassID() != SHAPE_CLASS_ID)
	{
		buildRemanenceError(this, node, "Can't get curves from %s");
		return NULL;
	}
	
	ShapeObject *so = (ShapeObject *) obj;	
	if (so->NumberOfCurves() != 1 || so->NumberOfPieces(time, 0) == 0)
	{
		buildRemanenceError(this, node, "Remanence export : %s should only contain one curve with at least one segment!");		
		return NULL;
	}

	int numPieces = so->NumberOfPieces(time, 0);
	srs->setNumCorners(numPieces + 1);

	// build offset matrix
	Matrix3 invNodeTM = node.GetNodeTM(time);
	invNodeTM.Invert();

	// Get the object matrix
	Matrix3 objectTM = node.GetObjectTM(time);

	// Compute the local to world matrix
	Matrix3 objectToLocal = objectTM*invNodeTM;	


	for(uint k = 0; k <= (uint) numPieces; ++k)
	{
		Point3 pos;		
		pos = (k == 0) ? so->InterpPiece3D(time, 0, 0, 0.f)
					   : so->InterpPiece3D(time, 0, k - 1, 1.f);		
		NLMISC::CVector nelPos;		
		convertVector(nelPos, objectToLocal * pos);
		srs->setCorner(k, nelPos);
	}

	
	srs->setTextureShifting(CExportNel::getScriptAppData (&node, NEL3D_APPDATA_REMANENCE_SHIFTING_TEXTURE, 0) != 0);	


	if (CExportNel::getScriptAppData (&node, NEL3D_APPDATA_EXPORT_ANIMATED_MATERIALS, 0) != 0)
	{
		srs->setAnimatedMaterial(mmbb.MaterialInfo[0].MaterialName);
	}					
	

	// ********************************
	// *** Export default transformation
	// ********************************

	// Get the node matrix
	Matrix3 localTM;
	getLocalMatrix (localTM, node, time);

	// Get the translation, rotation, scale of the node
	CVector pos, scale;
	CQuat rot;
	decompMatrix (scale, rot, pos, localTM);

	// Set the default values
	srs->getDefaultPos()->setDefaultValue(pos);					
	srs->getDefaultScale()->setDefaultValue(scale);					
	srs->getDefaultRotQuat()->setDefaultValue(rot);


	return srs.release();

	/*ObjectState os = node.EvalWorldState(_Ip->GetTime());		
//.........这里部分代码省略.........

	//---------------------------------------------------------------
	void ControllerExporter::exportSkinController( ExportNode* exportNode, SkinController* skinController, const String& controllerId, const String& skinSource )
	{
		INode* iNode = exportNode->getINode();

		if ( !skinController )
			return;

		// We cannot use skin->GetContextInterface to get ISkinContextData if we are exporting an XRef, since we cannot access
		// the INode the object belongs to in the referenced file. To solve this problem, we temporarily create an INode, that references
		// the object and delete it immediately. 
		ISkinInterface* skinInterface = getISkinInterface( exportNode, skinController );

		if ( !skinInterface )
			return;

		openSkin(controllerId, skinSource);
	
		Matrix3 bindShapeTransformationMatrix;
		skinInterface->getSkinInitTM(bindShapeTransformationMatrix, true);	
		double  bindShapeTransformationArray[4][4];
		VisualSceneExporter::matrix3ToDouble4x4(bindShapeTransformationArray, bindShapeTransformationMatrix);

		addBindShapeTransform(bindShapeTransformationArray);

		int jointCount = skinInterface->getNumBones();
		INodeList boneINodes;


		// Export joints source
		String jointsId = controllerId + JOINTS_SOURCE_ID_SUFFIX;
		COLLADASW::NameSource jointSource(mSW);
		jointSource.setId(jointsId);
		jointSource.setArrayId(jointsId + ARRAY_ID_SUFFIX);
		jointSource.setAccessorStride(1);
		jointSource.getParameterNameList().push_back("JOINT");
		jointSource.setAccessorCount(jointCount);
		jointSource.prepareToAppendValues();

		for (int i = 0; i <  jointCount; ++i)
		{
			// there should not be any null bone.
			// the ISkin::GetBone, not GetBoneFlat, function is called here.
			INode* boneNode = skinInterface->getBone(i);
			assert(boneNode);
			boneINodes.push_back(boneNode);

			ExportNode* jointExportNode = mExportSceneGraph->getExportNode(boneNode);
			assert(jointExportNode);

			if ( !jointExportNode->hasSid() )
				jointExportNode->setSid(mExportSceneGraph->createJointSid());

			jointExportNode->setIsJoint();

			jointSource.appendValues(jointExportNode->getSid());
		}
		jointSource.finish();

		determineReferencedJoints(exportNode, skinController);

		//export inverse bind matrix source
		String inverseBindMatrixId = controllerId + BIND_POSES_SOURCE_ID_SUFFIX;
		COLLADASW::Float4x4Source inverseBindMatrixSource(mSW);
		inverseBindMatrixSource.setId(inverseBindMatrixId);
		inverseBindMatrixSource.setArrayId(inverseBindMatrixId + ARRAY_ID_SUFFIX);
		inverseBindMatrixSource.setAccessorStride(16);
		inverseBindMatrixSource.getParameterNameList().push_back("TRANSFORM");
		inverseBindMatrixSource.setAccessorCount(jointCount);
		inverseBindMatrixSource.prepareToAppendValues();

		for (int i = 0; i <  jointCount; ++i)
		{
			INode* boneNode = boneINodes[i];

			Matrix3 bindPose;
			if ( !skinInterface->getBoneInitTM(boneNode, bindPose) )
			{
				bindPose = VisualSceneExporter::getWorldTransform( boneNode, mDocumentExporter->getOptions().getAnimationStart() );
			}
			bindPose.Invert();

			double bindPoseArray[4][4];
			VisualSceneExporter::matrix3ToDouble4x4(bindPoseArray, bindPose);
			inverseBindMatrixSource.appendValues(bindPoseArray);
		}
		inverseBindMatrixSource.finish();

		int vertexCount = skinInterface->getNumVertices();
		
		//count weights, excluding the ones equals one
		int weightsCount = 1;
		for (int i = 0; i < vertexCount; ++i)
		{
			int jointCount = skinInterface->getNumAssignedBones(i);
			for (int p = 0; p < jointCount; ++p)
			{
				float weight = skinInterface->getBoneWeight(i, p);
				if ( !COLLADASW::MathUtils::equals(weight, 1.0f) )
					weightsCount++;
//.........这里部分代码省略.........

//.........这里部分代码省略.........
    normalLoc = program.GetAttribLocation(Program::ATTRIB_NORMAL);
    if( Program::ATTRIB_UNKNOWN != normalLoc )
    {
      mContext->VertexAttribPointer( normalLoc, 3, GL_FLOAT, GL_FALSE, stride, &v->nX );
      mContext->EnableVertexAttributeArray( normalLoc );
    }
  }
  else if( mesh->GetMeshData(Mesh::RENDER_THREAD).HasColor() )  // Normals & color are mutually exclusive
  {
    colorLoc = program.GetAttribLocation(Program::ATTRIB_COLOR);
    if( Program::ATTRIB_UNKNOWN != colorLoc)
    {
      mContext->VertexAttribPointer( colorLoc, 3, GL_FLOAT, GL_FALSE, stride, &v->vertexR );
      mContext->EnableVertexAttributeArray( colorLoc );
    }
  }

  material.SetUniforms( mRenderMaterialUniforms, program, shaderType );

  if( mAffectedByLighting )
  {
    // Set light parameters
    location = mCustomUniform[ shaderType ][ 3 ].GetUniformLocation( program, "uNumberOfLights" );
    if( Program::UNIFORM_UNKNOWN != location )
    {
      program.SetUniform1i( location, mLightController->GetNumberOfLights() );
    }

    // Model View IT matrix required for vertex normal lighting calculation
    location = mCustomUniform[ shaderType ][ 4 ].GetUniformLocation( program, "uModelViewIT" );
    if( Program::UNIFORM_UNKNOWN != location )
    {
      Matrix3 modelViewInverseTranspose = modelViewMatrix;
      modelViewInverseTranspose.Invert();
      modelViewInverseTranspose.Transpose();

      program.SetUniformMatrix3fv( location, 1, modelViewInverseTranspose.AsFloat() );
    }

    // only one active light supported at the moment (due to performance)
    //if( numberOfLights > 0 )
    {
      Vector2 tempVector2;
      Vector3 tempVector3;

      Node& lightNode = mLightController->GetLight( 0 );

      LightAttachment& light = dynamic_cast< LightAttachment& >( lightNode.GetAttachment() );

      location = mCustomUniform[ shaderType ][ 5 ].GetUniformLocation( program, "uLight0.mType" );
      if( Program::UNIFORM_UNKNOWN != location )
      {
        program.SetUniform1i( location, (GLint)light.GetType() );
      }

      location = mCustomUniform[ shaderType ][ 6 ].GetUniformLocation( program, "uLight0.mFallOff" );
      if( Program::UNIFORM_UNKNOWN != location )
      {
        tempVector2 = light.GetFallOff();
        program.SetUniform2f( location, tempVector2.x, tempVector2.y );
      }

      location = mCustomUniform[ shaderType ][ 7 ].GetUniformLocation( program, "uLight0.mSpotAngle" );
      if( Program::UNIFORM_UNKNOWN != location )
      {
        tempVector2 = light.GetSpotAngle();

Void SoftBody::_SetupPose( Bool bIsVolume, Bool bIsFrame )
{
    const Node * pNode;
    UInt iNodeIndex;

    // Update links
    EnumLinks();
    Link * pLink = EnumNextLink();
    while( pLink != NULL ) {
        pLink->UpdateConstants();
        pLink = EnumNextLink();
    }

    // Compute total mass
    Scalar fTotalMass = _ComputeMass();
    Scalar fMassK = fTotalMass * 1000.0f * (Scalar)( GetNodeCount() );

    EnumNodes();
    pNode = EnumNextNode();
    while( pNode != NULL ) {
        if ( pNode->InvMass == 0.0f )
            fTotalMass += fMassK;
        pNode = EnumNextNode();
    }
    Scalar fInvTotalMass = MathFn->Invert( fTotalMass );

    // Flags
    m_hPose.bIsVolume = bIsVolume;
    m_hPose.bIsFrame = bIsFrame;

    // Weights
    m_hPose.arrWeights.Clear();

    EnumNodes();
    pNode = EnumNextNode();
    while( pNode != NULL ) {
        if ( pNode->InvMass > 0.0f )
            m_hPose.arrWeights.Push( pNode->Mass * fInvTotalMass );
        else
            m_hPose.arrWeights.Push( fMassK * fInvTotalMass );
        pNode = EnumNextNode();
    }

    // COM
    m_hPose.vCenterOfMass = _ComputeCenterOfMass();

    // Deltas
    m_hPose.arrDeltas.Clear();

    EnumNodes();
    pNode = EnumNextNode();
    while( pNode != NULL ) {
        m_hPose.arrDeltas.Push( pNode->Position - m_hPose.vCenterOfMass );
        pNode = EnumNextNode();
    }

    // Volume
    m_hPose.fVolume = ( (bIsVolume) ? _ComputeVolume() : 0.0f );

    // BaseScaling
    Matrix3 matBaseScaling;
    matBaseScaling.MakeNull();

    Vector3 vTmpRow;

    EnumNodes();
    pNode = EnumNextNode();
    while( pNode != NULL ) {
        iNodeIndex = pNode->GetIndex();
        const Vector3 & vDelta = m_hPose.arrDeltas[iNodeIndex];
        const Vector3 & vWeightedDelta = ( vDelta * m_hPose.arrWeights[iNodeIndex] );
        matBaseScaling.GetRow( vTmpRow, 0 );
        matBaseScaling.SetRow( 0, vTmpRow + (vDelta * vWeightedDelta.X) );
        matBaseScaling.GetRow( vTmpRow, 1 );
        matBaseScaling.SetRow( 1, vTmpRow + (vDelta * vWeightedDelta.Y) );
        matBaseScaling.GetRow( vTmpRow, 2 );
        matBaseScaling.SetRow( 2, vTmpRow + (vDelta * vWeightedDelta.Z) );
        pNode = EnumNextNode();
    }
    matBaseScaling.Invert( m_hPose.matBaseScaling );

    // Rotation & Scaling
    m_hPose.matRotation.MakeIdentity();
    m_hPose.matScaling.MakeIdentity();
}

//.........这里部分代码省略.........
							nlassert (res==MATRIX_RETURNED);

							// Add an entry inthe map
							if (boneBindPos.insert (mapBoneBindPos::value_type (bone, bindPos)).second)
							{
#ifdef NL_DEBUG
								// *** Debug info

								// Bone name
								std::string boneName=getName (*bone);

								// Local matrix
								Matrix3 nodeTM;
								nodeTM=bone->GetNodeTM (0);

								// Offset matrix
								Matrix3 offsetScaleTM (TRUE);
								Matrix3 offsetRotTM (TRUE);
								Matrix3 offsetPosTM (TRUE);
								ApplyScaling (offsetScaleTM, bone->GetObjOffsetScale ());
								offsetRotTM.SetRotate (bone->GetObjOffsetRot ());
								offsetPosTM.SetTrans (bone->GetObjOffsetPos ());
								Matrix3 offsetTM = offsetScaleTM * offsetRotTM * offsetPosTM;

								// Local + offset matrix
								Matrix3 nodeOffsetTM = offsetTM * nodeTM;

								// Init TM
								Matrix3 initTM;
								int res=physiqueInterface->GetInitNodeTM (bone, initTM);
								nlassert (res==MATRIX_RETURNED);

								// invert
								initTM.Invert();
								Matrix3 compNode=nodeTM*initTM;
								Matrix3 compOffsetNode=nodeOffsetTM*initTM;
								Matrix3 compOffsetNode2=nodeOffsetTM*initTM;
#endif // NL_DEBUG
							}
						}
						else
						{
							// It must be a blendable vertex
							nlassert (type==RIGID_BLENDED_TYPE);
							IPhyBlendedRigidVertex	*blendedInterface=(IPhyBlendedRigidVertex*)vertexInterface;

							// For each bones
							uint boneIndex;
							uint count=(uint)blendedInterface->GetNumberNodes ();
							for (boneIndex=0; boneIndex<count; boneIndex++)
							{
								// Get the bone pointer
								INode *bone = blendedInterface->GetNode(boneIndex);

								if (bone == NULL)
								{
									nlwarning("bone == NULL; boneIndex = %i / %i", boneIndex, count);
								}
								else
								{
									// Get the bind matrix of the bone
									Matrix3 bindPos;
									int res = physiqueInterface->GetInitNodeTM (bone, bindPos);

									if (res != MATRIX_RETURNED)
									{

//.........这里部分代码省略.........

		capsule->SetFirstPoint(TOVECTOR3(pt1)/scale);
		capsule->SetSecondPoint(TOVECTOR3(pt2)/scale);
		capsule->SetMaterial(HAV_MAT_SKIN);

		shape = StaticCast<bhkShape>(capsule);
		
	}
	else {
		// Sphere
		//CalcBoundingSphere(node, tm.GetTrans(), radius, 0);

		bhkSphereShapeRef sphere = new bhkSphereShape();
		sphere->SetRadius(radius/scale);
		sphere->SetMaterial(HAV_MAT_SKIN);
		shape = StaticCast<bhkShape>(sphere);
	}

	bhkRigidBodyRef body;

	if (shape)
	{
		bhkBlendCollisionObjectRef blendObj = new bhkBlendCollisionObject();
		body = new bhkRigidBody();

		Matrix3 tm = ragdollParent->GetObjTMAfterWSM(0);
		
		//Calculate Object Offset Matrix
		Matrix3 otm(1);
		Point3 pos = ragdollParent->GetObjOffsetPos();
		otm.PreTranslate(pos);
		Quat quat = ragdollParent->GetObjOffsetRot();
		PreRotateMatrix(otm, quat);
		Matrix3 otmInvert = otm;
		otmInvert.Invert();

		//correct object tm
		Matrix3 tmbhk = otmInvert * tm;

		//set geometric parameters
		body->SetRotation(TOQUATXYZW(Quat(tmbhk).Invert()));
		body->SetTranslation(TOVECTOR4(tmbhk.GetTrans() / scale));
		body->SetCenter(TOVECTOR4(ragdollParent->GetObjOffsetPos())/scale);

		//set physics
		if (IParamBlock2* rbParameters = rbMod->GetParamBlockByID(PB_RB_MOD_PBLOCK)) {
			//These are fundamental parameters

			int lyr = NP_DEFAULT_HVK_LAYER;
			int mtl = NP_DEFAULT_HVK_MATERIAL;
			int msys = NP_DEFAULT_HVK_MOTION_SYSTEM;
			int qtype = NP_DEFAULT_HVK_QUALITY_TYPE;
			float mass = NP_DEFAULT_HVK_MASS;
			float lindamp = NP_DEFAULT_HVK_LINEAR_DAMPING;
			float angdamp = NP_DEFAULT_HVK_ANGULAR_DAMPING;
			float frict = NP_DEFAULT_HVK_FRICTION;
			float maxlinvel = NP_DEFAULT_HVK_MAX_LINEAR_VELOCITY;
			float maxangvel = NP_DEFAULT_HVK_MAX_ANGULAR_VELOCITY;
			float resti = NP_DEFAULT_HVK_RESTITUTION;
			float pendepth = NP_DEFAULT_HVK_PENETRATION_DEPTH;
			Point3 InertiaTensor;


			rbParameters->GetValue(PA_RB_MOD_MASS, 0, mass, FOREVER);
			rbParameters->GetValue(PA_RB_MOD_RESTITUTION, 0, resti, FOREVER);
			rbParameters->GetValue(PA_RB_MOD_FRICTION, 0, frict, FOREVER);