C++ Matrix4::Identity方法代码示例

vec3_t *G2Exporter_Surface_GetVertNormal(int iSurfaceIndex, int iVertIndex, int iLODIndex)
{
	static vec3_t v3={0};
	memset(v3,0,sizeof(v3));

	if (iLODIndex == giNumLODs-1)
	{
		// q3data surface...
		//
		if (iSurfaceIndex < giNumSurfaces)
		{
			// standard surface...
			//
			md3SurfaceData_t *pSurfaceData = GetMD3SurfaceData(iSurfaceIndex);
			if (pSurfaceData)
			{
				// this logic is kinda gay, not sure why the *6 etc, but that's how other q3data code works, so...
				//
				float **ppVerts = pSurfaceData->verts;
				memcpy(v3,(vec3_t*) &ppVerts[0][iVertIndex*6+3], sizeof(v3));

				{		
					Matrix4 Swap;
							Swap.Identity();
					
					if (1)
					{
						Swap.SetRow(0,Vect3(0.0f,-1.0f,0.0f));
						Swap.SetRow(1,Vect3(1.0f,0.0f,0.0f));
					}
					Swap.CalcFlags();

					Vect3 v3In((const float *)v3);
					static Vect3 v3Out;
					Swap.XFormVect(v3Out,v3In);
					return (vec3_t*) &v3Out;
				}
			}
		}
		else
		{
			// tag surface...
			//
			// I don't think tag-surfaces normals have any meaning for ghoul2, so...
			//		
			return &v3;
		}
	}
	else
	{
		// imported surface...
		//
		vec3_t &v3Src = ImportedModel.ImportedLODs[iLODIndex].ImportedSurfaces[ImportedModel.SurfaceIndexRemaps[iSurfaceIndex]].ImportedVerts[iVertIndex].normal;
		memcpy(v3,v3Src,sizeof(v3));
		return &v3;
	}

	assert(0);	
	return &v3;
}

	void TranslateMatrix4(const Vector3 &v, Matrix4 &result)
	{
		result.Identity();

		result.wX = v.x;
		result.wY = v.y;
		result.wZ = v.z;
	}

 MATHDLL_API Vector3 RotateAround(const Vector3 & axis, float amount, const Vector3 & vectorToRotate)
 {
   Matrix4 identity;
   identity.Identity();
   Matrix4 first   = identity * cos(amount);
   Matrix4 second  = TensorProductMatrix(axis) * (1 - cos(amount));
   Matrix4 third   = CrossProductMatrix(axis) * sin(amount);
   Vector4 endVal = (first + second + third) * vectorToRotate;
   return Vector3(endVal.x, endVal.y, endVal.z);
 }

TEST(matrix4, identitymatrix)
{
	Matrix4 m1(1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, 1, 0,
		0, 0, 0, 1);

	Matrix4 iD;
	iD.Identity();
	EXPECT_TRUE(m1 == iD);
}

void Matrix4::Rotate(Vector3 r)
{
  Matrix4 m;
  float rx=DEGTORAD*r.x;
  float ry=DEGTORAD*r.y;
  float rz=DEGTORAD*r.z;

  m[1][1]=cos(rx);  m[1][2]=sin(rx);   
  m[2][1]=-sin(rx); m[2][2]=cos(rx);
  *this=(*this)*m;

  m.Identity(); 
  m[0][0]=cos(ry);  m[0][1]=sin(ry);   
  m[2][1]=-sin(ry); m[2][2]=cos(ry);
  *this=(*this)*m;

  m.Identity(); 
  m[0][0]=cos(rz);  m[0][1]=sin(rz);   
  m[1][0]=-sin(rz); m[1][1]=cos(rz);
  *this=(*this)*m;
}

	void CardinalOrthoProject(bool xAxis, bool yAxis, bool zAxis, Matrix4 &result)
	{
		result.Identity();
		if (xAxis)
			result.xX = 0;

		if (yAxis)
			result.yY = 0;

		if (zAxis)
			result.zZ = 0;
	}

void EditorBodyControl::ApplyTransform(float32 x, float32 y, float32 z)
{
	if (!InModificationMode())
		return;

    Entity *selectedNode = scene->GetProxy();
    if(selectedNode)
	{
		Matrix4 modification;
		modification.Identity();

		Matrix4 t1, t2;
		t1.CreateTranslation(-selectedNode->GetWorldTransform().GetTranslationVector());
		t2.CreateTranslation(selectedNode->GetWorldTransform().GetTranslationVector());

		switch (GetModificationMode())
		{
			case ResourceEditor::MODIFY_MOVE:
				modification.CreateTranslation(Vector3(x, y, z));
				break;

			case ResourceEditor::MODIFY_ROTATE:
				modification.CreateRotation(Vector3(1, 0, 0), DegToRad(x));
				modification *= Matrix4::MakeRotation(Vector3(0, 1, 0), DegToRad(y));
				modification *= Matrix4::MakeRotation(Vector3(0, 0, 1), DegToRad(z));

				modification = (t1 * modification) * t2;
				break;

			case ResourceEditor::MODIFY_SCALE:
				modification.CreateScale(Vector3(1, 1, 1) + Vector3(x + y + z, x + y + z, x + y + z) / 100.f);
				modification = (t1 * modification) * t2;
				break;

			default:
				break;
		}

		Matrix4 originalTransform = selectedNode->GetLocalTransform();
		modification = originalTransform * modification;

		if (IsLandscapeRelative())
		{
			modification = modification * GetLandscapeOffset(modification);
		}

		CommandsManager::Instance()->ExecuteAndRelease(new CommandTransformObject(selectedNode,
																				  originalTransform,
																				  modification),
													   scene);
	}
}

Matrix4 RPhysics::GetOrientTransform(Vector4 dirStart, Vector4 upStart1, Vector4 dirEnd, Vector4 upEnd1) {
    Vector4 temp = CrossProduct(dirStart,upStart1);
    Vector4 upStart = CrossProduct(temp,dirStart);
    temp = CrossProduct(dirEnd,upEnd1);
    Vector4 upEnd = CrossProduct(temp,dirEnd);
    dirStart = Normalize3(dirStart);
    dirEnd = Normalize3(dirEnd);
    upStart = Normalize3(upStart);
    upEnd = Normalize3(upEnd);
    Matrix4 transform;
    transform.Identity();
    
    Vector4 axis1 = CrossProduct(dirStart,dirEnd);
    if(Length3(axis1)==0) axis1 = upEnd;
    float angle1 = Angle3(dirStart,dirEnd);
    Matrix4 rotMat;
    rotMat.Rotation(angle1,axis1);
    Vector4 newUp = rotMat*upStart;
    Vector4 axis2 = CrossProduct(newUp,upEnd);
    if(Length3(axis2)==0) axis2 = dirEnd;
    float angle2 = Angle3(upEnd,newUp);
    if(angle1*angle2*0!=0) return transform;
    Matrix4 toRot;
    toRot.Rotation(angle2,axis2);
    transform= transform*toRot;
    toRot.Rotation(angle1,axis1);
    transform = transform*toRot;
    if(!(transform[0][0]<=3||transform[0][0]>=3)) {
        cerr<<endl;
        cerr<<angle1<<endl;
        cerr<<angle2<<endl;

        PrintVector(dirStart);
        PrintVector(upStart);
        PrintVector(dirEnd);
        PrintVector(upEnd);
        cout<<flush;
        exit(1);
    }
    return transform;

    
}

Matrix4 EditorBodyControl::GetLandscapeOffset(const Matrix4& transform)
{
	Matrix4 resTransform;
	resTransform.Identity();

	Landscape* landscape = FindLandscape(scene);
	if(!landscape) return resTransform;

	Vector3 p = Vector3(0, 0, 0) * transform;

	Vector3 result;
	bool res = landscape->PlacePoint(p, result);
	if (res)
	{
		Vector3 offset = result - p;
		resTransform.CreateTranslation(offset);
	}

	return resTransform;
}

	void RotateMatrix4Y(float angle, Matrix4 &result)
	{
		// Y axis rotation 
		// /--           --\
		// | cosT  0 -sinT |
		// | 0     1    0  |
		// | sinT  0  cosT |
		// \--           --/
		result.Identity();

		// Convert angle to radian
		float theta = ToRadian(angle);
		float sine, cosine;
		SinCos(cosine, sine, theta);

		// X axis rotation
		result.xX = cosine;
		result.xZ = -sine;

		// Z axis rotation
		result.zX = sine;
		result.zZ = cosine;
	}

void Matrix4::Uviewpoint(const Coord3D& v1,
                         const Coord3D& v2,
                         const Coord3D& up)
{
   // find the vector that points in the v21 direction
   Coord3D v_hat_21 = v2 - v1;

   // compute rotation matrix that takes -z axis to the v21 axis,
   // and y to the up direction
   Matrix4 rotMat;
   rotMat.UviewDirection(v_hat_21, up);

   // build matrix that translates the origin to v1
   Matrix4 transMat;
   transMat.Identity();
   transMat.m[3][0] = v1.cX;
   transMat.m[3][1] = v1.cY;
   transMat.m[3][2] = v1.cZ;

   // concatenate the matrices together
   MatrixProduct(rotMat, transMat);

}

void Matrix4::UviewDirection(const Coord3D& v21,
                             const Coord3D& up)
{
   double sine, cosine;

    // find the unit vector that points in the v21 direction
    Coord3D v_hat_21(v21);
    double len = v_hat_21.Magnitude();

    Matrix4 amat;
    if (fabs(len) > stdEps)
    {
        len = 1.0 / len;
        v_hat_21 *= len;

        // rotate z in the xz-plane until same latitude
        sine = sqrt (1.0 - v_hat_21.cZ * v_hat_21.cZ);

        amat.RotateY(-v_hat_21.cZ, -sine);

    }
    else
        // error condition: zero length vecotr passed in -- do nothing */
        amat.Identity();

    // project v21 onto the xy plane
    Coord3D v_xy(v21);
    v_xy.cZ = 0.0;
    len = v_xy.Magnitude();

    // rotate in the x-y plane until v21 lies on z axis ---
    // but of course, if its already there, do nothing
    Matrix4 bmat, cmat;
    if (fabs(len) > stdEps)
    {
      // want xy projection to be unit vector, so that sines/cosines pop out
      len = 1.0 / len;
      v_xy *= len;

      // rotate the projection of v21 in the xy-plane over to the x axis
      bmat.RotateZ(v_xy.cX, v_xy.cY);

      // concatenate these together
      cmat.MatrixProduct(amat, bmat);

    }
    else
        cmat = amat;


    /* up vector really should be perpendicular to the x-form direction --
     * Use up a couple of cycles, and make sure it is,
     * just in case the user blew it.
     */
    Coord3D up_proj;
    up_proj.Perpendicular(up, v_hat_21);
    len = up_proj.Magnitude();
    if (fabs(len) > stdEps)
    {
        // normalize the vector
        len = 1.0/len;
        up_proj *= len;

        // compare the up-vector to the  y-axis to get the cosine of the angle
        Coord3D tmp;
        tmp.cX = cmat.m[1][0];
        tmp.cY = cmat.m[1][1];
        tmp.cZ = cmat.m[1][2];
        cosine = tmp.Dot(up_proj);

        // compare the up-vector to the x-axis to get the sine of the angle
        tmp.cX = cmat.m[0][0];
        tmp.cY = cmat.m[0][1];
        tmp.cZ = cmat.m[0][2];
        sine = tmp.Dot(up_proj);

        // rotate to align the up vector with the y-axis
        amat.RotateZ(cosine, -sine);

        // This xform, although computed last, acts first
        MatrixProduct(amat, cmat);

    }
    else
    {

        // error condition: up vector is indeterminate (zero length)
        // -- do nothing
        *this = cmat;
    }
}

vec3_t *G2Exporter_Surface_GetVertCoords(int iSurfaceIndex, int iVertIndex, int iLODIndex)
{
	static vec3_t v3={0};
	memset(&v3,0,sizeof(v3));

	if (iLODIndex == giNumLODs-1)
	{
		// q3data surface...
		//
		if (iSurfaceIndex < giNumSurfaces)
		{
			// standard surface...
			//
			md3SurfaceData_t *pSurfaceData = GetMD3SurfaceData(iSurfaceIndex);
			if (pSurfaceData)
			{
				// this logic is kinda gay, not sure why the *6 etc, but that's how other q3data code works, so...
				//
				float **ppVerts = pSurfaceData->verts;
				static vec3_t v3;
				for (int i=0; i<3; i++)
				{
					v3[i] = ppVerts[0][iVertIndex*6+i];// /MD3_XYZ_SCALE;
				}
				// return &v3;
					
				Matrix4 Swap;
						Swap.Identity();
				
				if (1)
				{
					Swap.SetRow(0,Vect3(0.0f,-1.0f,0.0f));
					Swap.SetRow(1,Vect3(1.0f,0.0f,0.0f));
				}
				Swap.CalcFlags();

				Vect3 v3In((const float *)v3);
				static Vect3 v3Out;
				Swap.XFormVect(v3Out,v3In);
				return (vec3_t*) &v3Out;
			}
		}
		else
		{
			// tag surface...
			//
			assert(iVertIndex<3);

			md3Tag_t *pTag = &g_data.tags[0][iSurfaceIndex - giNumSurfaces];			
			vec3_t v3New;

	//#ifdef PERFECT_CONVERSION
			
			v3New[0] = pTag->axis[0][iVertIndex] ;
			v3New[1] = pTag->axis[1][iVertIndex] ;
			v3New[2] = pTag->axis[2][iVertIndex] ;

			// don't worry about how this crap works, it just does (arrived at by empirical methods... :-)
			//
			//  (mega-thanks to Gil as usual)
			//
			if (iVertIndex==2)
			{
				VectorCopy(pTag->origin,v3);
			}
			else
			if (iVertIndex==1)
			{
				v3New[0] =   2.0f * pTag->axis[1][iG2_TRISIDE_MIDDLE];
				v3New[1] = -(2.0f * pTag->axis[0][iG2_TRISIDE_MIDDLE]);
				v3New[2] =   2.0f * pTag->axis[2][iG2_TRISIDE_MIDDLE];
				
				VectorSubtract(pTag->origin,v3New,v3);
			}
			else
			{					
				v3New[0] =  pTag->axis[1][iG2_TRISIDE_LONGEST];
				v3New[1] = -pTag->axis[0][iG2_TRISIDE_LONGEST];
				v3New[2] =  pTag->axis[2][iG2_TRISIDE_LONGEST];
				
				VectorSubtract(pTag->origin,v3New,v3);
			}

	//		return (vec3_t*) &v3;

			Matrix4 Swap;
					Swap.Identity();
			
			if (1)
			{
				Swap.SetRow(0,Vect3(0.0f,-1.0f,0.0f));
				Swap.SetRow(1,Vect3(1.0f,0.0f,0.0f));
			}
			Swap.CalcFlags();

			Vect3 v3In((const float *)v3);
			static Vect3 v3Out;
			Swap.XFormVect(v3Out,v3In);

			return (vec3_t*) &v3Out;
//.........这里部分代码省略.........

void EditorBodyControl::PrepareModMatrix(const Vector2 & point)
{
	float32 winx = point.x - touchStart.x;
	float32 winy = point.y - touchStart.y;
	
	Matrix4 modification;
	modification.Identity();

	ArrowsNode* arrowsNode = GetArrowsNode(false);
	if (!arrowsNode)
		return;

	if (GetModificationMode() == ResourceEditor::MODIFY_MOVE)
	{
		Vector3 from, dir;
		GetCursorVectors(&from, &dir, point);
		
		Vector3 currPoint;
		bool result = GetIntersectionVectorWithPlane(from, dir, planeNormal, rotationCenter, currPoint);
		
		if (result)
		{
			
			if (arrowsNode)
			{
				switch (arrowsNode->GetModAxis())
				{
					case ArrowsNode::AXIS_X:
						currPoint.y = startDragPoint.y;
						currPoint.z = startDragPoint.z;
						break;
					case ArrowsNode::AXIS_Y:
						currPoint.x = startDragPoint.x;
						currPoint.z = startDragPoint.z;
						break;
					case ArrowsNode::AXIS_Z:
						currPoint.x = startDragPoint.x;
						currPoint.y = startDragPoint.y;
						break;
                    
					default:
						break;
				}
				modification.CreateTranslation(currPoint - startDragPoint);
			}
		}
	}
	else if (GetModificationMode() == ResourceEditor::MODIFY_ROTATE)
	{
		Matrix4 d;
		switch (arrowsNode->GetModAxis())
		{
			case ArrowsNode::AXIS_X:
			case ArrowsNode::AXIS_Y:
				modification.CreateRotation(vect[arrowsNode->GetModAxis()], winy / 100.0f);
				break;
			case ArrowsNode::AXIS_Z:
				modification.CreateRotation(vect[arrowsNode->GetModAxis()], winx / 100.0f);
				break;
			case ArrowsNode::AXIS_XY:
				modification.CreateRotation(vect[ArrowsNode::AXIS_X], winx / 100.0f);
				d.CreateRotation(vect[ArrowsNode::AXIS_Y], winy / 100.0f);
				modification *= d;
				break;
			case ArrowsNode::AXIS_YZ:
				modification.CreateRotation(vect[ArrowsNode::AXIS_Y], winx / 100.0f);
				d.CreateRotation(vect[ArrowsNode::AXIS_Z], winy / 100.0f);
				modification *= d;
				break;
			case ArrowsNode::AXIS_XZ:
				modification.CreateRotation(vect[ArrowsNode::AXIS_X], winx / 100.0f);
				d.CreateRotation(vect[ArrowsNode::AXIS_Z], winy / 100.0f);
				modification *= d;
				break;
			default:
				break;
		}
		modification = (translate1 * modification) * translate2;
		
	}
	else if (GetModificationMode() == ResourceEditor::MODIFY_SCALE)
	{
		float kf = winx / 100.0f;
		if (kf < -1.0)
			kf = - kf - 2.0;
		modification.CreateScale(Vector3(1,1,1) + Vector3(1,1,1) * kf);
		modification = (translate1 * modification) * translate2;
	}
	currTransform = startTransform * modification;
}