C++ MatrixF::mulP方法代码示例

void GjkCollisionState::reset(const MatrixF& a2w, const MatrixF& b2w)
{
   VectorF zero(0,0,0),sa,sb;
   a2w.mulP(a->support(zero),&sa);
   b2w.mulP(b->support(zero),&sb);
   v = sa - sb;
   dist = v.len();
}

void BoxConvex::emitEdge(S32 v1,S32 v2,const MatrixF& mat,ConvexFeature* cf)
{
   S32 vc = cf->mVertexList.size();
   cf->mVertexList.increment(2);
   Point3F *vp = cf->mVertexList.begin();
   mat.mulP(getVertex(v1),&vp[vc]);
   mat.mulP(getVertex(v2),&vp[vc + 1]);

   cf->mEdgeList.increment();
   ConvexFeature::Edge& edge = cf->mEdgeList.last();
   edge.vertex[0] = vc;
   edge.vertex[1] = vc + 1;
}

void GFXDrawUtil::_drawSolidCapsule( const GFXStateBlockDesc &desc, const Point3F &center, F32 radius, F32 height, const ColorI &color, const MatrixF *xfm )
{	
   MatrixF mat;
   if ( xfm )
      mat = *xfm;      
   else
      mat = MatrixF::Identity;

   S32 numPoints = sizeof(circlePoints)/sizeof(Point2F);
   GFXVertexBufferHandle<GFXVertexPC> verts(mDevice, numPoints * 2 + 2, GFXBufferTypeVolatile);
   verts.lock();

   for (S32 i=0; i<numPoints + 1; i++)
   {
      S32 imod = i % numPoints;      
      verts[2 * i].point = Point3F( circlePoints[imod].x * radius, circlePoints[imod].y * radius, height );
      verts[2 * i].color = color;
      verts[2 * i + 1].point = Point3F( circlePoints[imod].x * radius, circlePoints[imod].y * radius, -height );
      verts[2 * i + 1].color = color;
   }

   S32 totalNumPnts = numPoints * 2 + 2;

   // Apply xfm if we were passed one.
   for ( U32 i = 0; i < totalNumPnts; i++ )
      mat.mulP( verts[i].point );

   // Apply position offset
   for ( U32 i = 0; i < totalNumPnts; i++ )
      verts[i].point += center;

   verts.unlock();

   mDevice->setStateBlockByDesc( desc );

   mDevice->setVertexBuffer( verts );
   mDevice->setupGenericShaders();

   mDevice->drawPrimitive( GFXTriangleStrip, 0, 2 * numPoints );

   Point3F sphereCenter;
   MatrixF sphereMat;

   if ( xfm )
      sphereMat = *xfm;
   else
      sphereMat = MatrixF::Identity;   

   sphereCenter.set( 0, 0, 0.5f * height );
   mat.mulV( sphereCenter );
   sphereCenter += center;

   drawSphere( desc, radius, sphereCenter, color, true, false, &sphereMat );

   sphereCenter.set( 0, 0, -0.5f * height );
   mat.mulV( sphereCenter );
   sphereCenter += center;

   drawSphere( desc, radius, sphereCenter, color, false, true, &sphereMat );
}

void BoxConvex::emitFace(S32 fi,const MatrixF& mat,ConvexFeature* cf)
{
   Face& face = sFace[fi];

   // Emit vertices
   S32 vc = cf->mVertexList.size();
   cf->mVertexList.increment(4);
   Point3F *vp = cf->mVertexList.begin();
   for (S32 v = 0; v < 4; v++)
      mat.mulP(getVertex(face.vertex[v]),&vp[vc + v]);

   // Emit edges
   cf->mEdgeList.increment(4);
   ConvexFeature::Edge* edge = cf->mEdgeList.end() - 4;
   for (S32 e = 0; e < 4; e++) {
      edge[e].vertex[0] = vc + e;
      edge[e].vertex[1] = vc + ((e + 1) & 3);
   }

   // Emit 2 triangle faces
   cf->mFaceList.increment(2);
   ConvexFeature::Face* ef = cf->mFaceList.end() - 2;
   mat.getColumn(face.axis,&ef->normal);
   if (face.flip)
      ef[0].normal.neg();
   ef[1].normal = ef[0].normal;
   ef[1].vertex[0] = ef[0].vertex[0] = vc;
   ef[1].vertex[1] = ef[0].vertex[2] = vc + 2;
   ef[0].vertex[1] = vc + 1;
   ef[1].vertex[2] = vc + 3;
}

void BtBody::applyImpulse( const Point3F &origin, const Point3F &force )
{
   AssertFatal( mActor, "BtBody::applyImpulse - The actor is null!" );
   AssertFatal( isDynamic(), "BtBody::applyImpulse - This call is only for dynamics!" );

   // Convert the world position to local
   MatrixF trans = btCast<MatrixF>( mActor->getCenterOfMassTransform() );
   trans.inverse();
   Point3F localOrigin( origin );
   trans.mulP( localOrigin );

   if ( mCenterOfMass )
   {
      Point3F relOrigin( localOrigin );
      mCenterOfMass->mulP( relOrigin );
      Point3F relForce( force );
      mCenterOfMass->mulV( relForce );
      mActor->applyImpulse( btCast<btVector3>( relForce ), btCast<btVector3>( relOrigin ) );
   }
   else
      mActor->applyImpulse( btCast<btVector3>( force ), btCast<btVector3>( localOrigin ) );

   if ( !mActor->isActive() )
      mActor->activate();
}

//----------------------------------------------------------------------------
// Create ring
//----------------------------------------------------------------------------
SplashRing Splash::createRing()
{
   SplashRing ring;
   U32 numPoints = mDataBlock->numSegments + 1;

   Point3F ejectionAxis( 0.0, 0.0, 1.0 );

   Point3F axisx;
   if (mFabs(ejectionAxis.z) < 0.999f)
      mCross(ejectionAxis, Point3F(0, 0, 1), &axisx);
   else
      mCross(ejectionAxis, Point3F(0, 1, 0), &axisx);
   axisx.normalize();

   for( U32 i=0; i<numPoints; i++ )
   {
      F32 t = F32(i) / F32(numPoints);

      AngAxisF thetaRot( axisx, mDataBlock->ejectionAngle * (M_PI / 180.0));
      AngAxisF phiRot(   ejectionAxis, t * (M_PI * 2.0));

      Point3F pointAxis = ejectionAxis;

      MatrixF temp;
      thetaRot.setMatrix(&temp);
      temp.mulP(pointAxis);
      phiRot.setMatrix(&temp);
      temp.mulP(pointAxis);

      Point3F startOffset = axisx;
      temp.mulV( startOffset );
      startOffset *= mDataBlock->startRadius;

      SplashRingPoint point;
      point.position = getPosition() + startOffset;
      point.velocity = pointAxis * mDataBlock->velocity;

      ring.points.push_back( point );
   }

   ring.color = mDataBlock->colors[0];
   ring.lifetime = mDataBlock->ringLifetime;
   ring.elapsedTime = 0.0;
   ring.v = mDataBlock->texFactor * mFmod( mElapsedTime, 1.0 );

   return ring;
}

bool GjkCollisionState::intersect(const MatrixF& a2w, const MatrixF& b2w)
{
   num_iterations = 0;
   MatrixF w2a,w2b;

   w2a = a2w;
   w2b = b2w;
   w2a.inverse();
   w2b.inverse();
   reset(a2w,b2w);

   bits = 0;
   all_bits = 0;

   do {
      nextBit();

      VectorF va,sa;
      w2a.mulV(-v,&va);
      p[last] = a->support(va);
      a2w.mulP(p[last],&sa);

      VectorF vb,sb;
      w2b.mulV(v,&vb);
      q[last] = b->support(vb);
      b2w.mulP(q[last],&sb);

      VectorF w = sa - sb;
      if (mDot(v,w) > 0)
         return false;
      if (degenerate(w)) {
         ++num_irregularities;
         return false;
      }

      y[last] = w;
      all_bits = bits | last_bit;

      ++num_iterations;
      if (!closest(v) || num_iterations > sIteration) {
         ++num_irregularities;
         return false;
      }
   }
   while (bits < 15 && v.lenSquared() > sEpsilon2);
   return true;
}

void TerrainConvex::getFeatures(const MatrixF& mat,const VectorF& n, ConvexFeature* cf)
{
   U32 i;
   cf->material = 0;
   cf->object = mObject;

   // Plane is normal n + support point
   PlaneF plane;
   plane.set(support(n),n);
   S32 vertexCount = cf->mVertexList.size();

   // Emit vertices on the plane
   S32* vertexListPointer;
   if (halfA)
      vertexListPointer = square ? sVertexList[(split45 << 1) | 1]: sVertexList[4];
   else
      vertexListPointer = square ? sVertexList[(split45 << 1)]    : sVertexList[4];

   S32 pm = 0;
   S32 numVerts = *vertexListPointer;
   vertexListPointer += 1;
   for (i = 0; i < numVerts; i++)
   {
      Point3F& cp = point[vertexListPointer[i]];
      cf->mVertexList.increment();
      mat.mulP(cp,&cf->mVertexList.last());
      pm |= 1 << vertexListPointer[i];
   }

   // Emit Edges
   S32* ep = (square && halfA)?
      (split45 ? sEdgeList45A[pm]: sEdgeList135A[pm]):
      (split45 ? sEdgeList45[pm]: sEdgeList135[pm]);

   S32 numEdges = *ep;
   S32 edgeListStart = cf->mEdgeList.size();
   cf->mEdgeList.increment(numEdges);
   ep += 1;
   for (i = 0; i < numEdges; i++)
   {
      cf->mEdgeList[edgeListStart + i].vertex[0] = vertexCount + ep[i * 2 + 0];
      cf->mEdgeList[edgeListStart + i].vertex[1] = vertexCount + ep[i * 2 + 1];
   }

   // Emit faces
   S32* fp = split45 ? sFaceList45[pm]: sFaceList135[pm];
   S32 numFaces = *fp;
   fp += 1;
   S32 faceListStart = cf->mFaceList.size();
   cf->mFaceList.increment(numFaces);
   for (i = 0; i < numFaces; i++)
   {
      cf->mFaceList[faceListStart + i].normal    = normal[fp[i * 4 + 0]];
      cf->mFaceList[faceListStart + i].vertex[0] = vertexCount + fp[i * 4 + 1];
      cf->mFaceList[faceListStart + i].vertex[1] = vertexCount + fp[i * 4 + 2];
      cf->mFaceList[faceListStart + i].vertex[2] = vertexCount + fp[i * 4 + 3];
   }
}

void GjkCollisionState::getCollisionInfo(const MatrixF& mat, Collision* info)
{
   AssertFatal(false, "GjkCollisionState::getCollisionInfo() - There remain scaling problems here.");
   // This assumes that the shapes do not intersect
   Point3F pa,pb;
   if (bits) {
      getClosestPoints(pa,pb);
      mat.mulP(pa,&info->point);
      b->getTransform().mulP(pb,&pa);
      info->normal = info->point - pa;
   }
   else {
      mat.mulP(p[last],&info->point);
      info->normal = v;
   }
   info->normal.normalize();
   info->object = b->getObject();
}

void ForestConvex::getFeatures( const MatrixF &mat, const VectorF &n, ConvexFeature *cf )
{
   cf->material = 0;
   cf->object = mObject;

   TSShapeInstance *si = mData->getShapeInstance();

   TSShape::ConvexHullAccelerator* pAccel =
      si->getShape()->getAccelerator(mData->getCollisionDetails()[hullId]);
   AssertFatal(pAccel != NULL, "Error, no accel!");

   F32 currMaxDP = mDot(pAccel->vertexList[0], n);
   U32 index = 0;
   U32 i;
   for (i = 1; i < pAccel->numVerts; i++) 
   {
      F32 dp = mDot(pAccel->vertexList[i], n);
      if (dp > currMaxDP) 
      {
         currMaxDP = dp;
         index = i;
      }
   }

   const U8* emitString = pAccel->emitStrings[index];
   U32 currPos = 0;
   U32 numVerts = emitString[currPos++];
   for (i = 0; i < numVerts; i++) 
   {
      cf->mVertexList.increment();
      U32 index = emitString[currPos++];
      mat.mulP(pAccel->vertexList[index], &cf->mVertexList.last());
   }

   U32 numEdges = emitString[currPos++];
   for (i = 0; i < numEdges; i++) 
   {
      U32 ev0 = emitString[currPos++];
      U32 ev1 = emitString[currPos++];
      cf->mEdgeList.increment();
      cf->mEdgeList.last().vertex[0] = ev0;
      cf->mEdgeList.last().vertex[1] = ev1;
   }

   U32 numFaces = emitString[currPos++];
   for (i = 0; i < numFaces; i++) 
   {
      cf->mFaceList.increment();
      U32 plane = emitString[currPos++];
      mat.mulV(pAccel->normalList[plane], &cf->mFaceList.last().normal);
      for (U32 j = 0; j < 3; j++)
         cf->mFaceList.last().vertex[j] = emitString[currPos++];
   }
}

//----------------------------------------------------------------------------
AwTextureTarget *AwShape::processAwesomiumHit (const Point3F &start, const Point3F &end)
{
	if (!mTextureTarget)
	{
		return NULL;
	}

	Point3F localStart, localEnd;
	MatrixF mat = getTransform();
	mat.scale (Point3F (getScale ()));
	mat.inverse ();

	mat.mulP (start, &localStart);
	mat.mulP (end, &localEnd);

	RayInfo info;
	info.generateTexCoord = true;
	if (!mShapeInstance || !mShapeInstance->castRayOpcode (0, localStart, localEnd, &info))
	{
		return NULL;
	}

	if (info.texCoord.x != -1 && info.texCoord.y != -1 && info.material == mMatInstance)
	{
		Point2I pnt (info.texCoord.x * mTextureTarget->getResolution ().x, info.texCoord.y * mTextureTarget->getResolution ().y);
		
		AwManager::sCursor->setPosition (pnt);

		if (mIsMouseDown)
		{
			mTextureTarget->injectMouseDown ();
		}
		else
		{
			mTextureTarget->injectMouseUp ();
		}

		return mTextureTarget;
	}
	return NULL;
}

MatrixF PlaneReflector::getFrustumClipProj( MatrixF &modelview )
{
   static MatrixF rotMat(EulerF( static_cast<F32>(M_PI / 2.f), 0.0, 0.0));
   static MatrixF invRotMat(EulerF( -static_cast<F32>(M_PI / 2.f), 0.0, 0.0));


   MatrixF revModelview = modelview;
   revModelview = rotMat * revModelview;  // add rotation to modelview because it needs to be removed from projection

   // rotate clip plane into modelview space
   Point4F clipPlane;
   Point3F pnt = refplane * -(refplane.d + 0.0 );
   Point3F norm = refplane;

   revModelview.mulP( pnt );
   revModelview.mulV( norm );
   norm.normalize();

   clipPlane.set( norm.x, norm.y, norm.z, -mDot( pnt, norm ) );


   // Manipulate projection matrix
   //------------------------------------------------------------------------
   MatrixF proj = GFX->getProjectionMatrix();
   proj.mul( invRotMat );  // reverse rotation imposed by Torque
   proj.transpose();       // switch to row-major order

   // Calculate the clip-space corner point opposite the clipping plane
   // as (sgn(clipPlane.x), sgn(clipPlane.y), 1, 1) and
   // transform it into camera space by multiplying it
   // by the inverse of the projection matrix
   Vector4F	q;
   q.x = sgn(clipPlane.x) / proj(0,0);
   q.y = sgn(clipPlane.y) / proj(1,1);
   q.z = -1.0F;
   q.w = ( 1.0F - proj(2,2) ) / proj(3,2);

   F32 a = 1.0 / (clipPlane.x * q.x + clipPlane.y * q.y + clipPlane.z * q.z + clipPlane.w * q.w);

   Vector4F c = clipPlane * a;

   // CodeReview [ags 1/23/08] Come up with a better way to deal with this.
   if(GFX->getAdapterType() == OpenGL)
      c.z += 1.0f;

   // Replace the third column of the projection matrix
   proj.setColumn( 2, c );
   proj.transpose(); // convert back to column major order
   proj.mul( rotMat );  // restore Torque rotation

   return proj;
}

void AppMesh::computeBounds(Box3F& bounds)
{
   bounds = Box3F::Invalid;

   if ( isSkin() )
   {
      // Need to skin the mesh before we can compute the bounds

      // Setup bone transforms
      Vector<MatrixF> boneTransforms;
      boneTransforms.setSize( nodeIndex.size() );
      for (S32 iBone = 0; iBone < boneTransforms.size(); iBone++)
      {
         MatrixF nodeMat = bones[iBone]->getNodeTransform( TSShapeLoader::DefaultTime );
         TSShapeLoader::zapScale(nodeMat);
         boneTransforms[iBone].mul( nodeMat, initialTransforms[iBone] );
      }

      // Multiply verts by weighted bone transforms
      for (S32 iVert = 0; iVert < initialVerts.size(); iVert++)
         points[iVert].set( Point3F::Zero );

      for (S32 iWeight = 0; iWeight < vertexIndex.size(); iWeight++)
      {
         const S32& vertIndex = vertexIndex[iWeight];
         const MatrixF& deltaTransform = boneTransforms[ boneIndex[iWeight] ];

         Point3F v;
         deltaTransform.mulP( initialVerts[vertIndex], &v );
         v *= weight[iWeight];

         points[vertIndex] += v;
      }

      // compute bounds for the skinned mesh
      for (S32 iVert = 0; iVert < initialVerts.size(); iVert++)
         bounds.extend( points[iVert] );
   }
   else
   {
      MatrixF transform = getMeshTransform(TSShapeLoader::DefaultTime);
      TSShapeLoader::zapScale(transform);

      for (S32 iVert = 0; iVert < points.size(); iVert++)
      {
         Point3F p;
         transform.mulP(points[iVert], &p);
         bounds.extend(p);
      }
   }
}

void OrientedBox3F::set( const MatrixF& transform, const Box3F& aabb )
{
   mCenter = aabb.getCenter();
   transform.mulP( mCenter );

   mAxes[ RightVector ] = transform.getRightVector();
   mAxes[ ForwardVector ] = transform.getForwardVector();
   mAxes[ UpVector ] = transform.getUpVector();

   mHalfExtents[ 0 ] = aabb.len_x() / 2.f;
   mHalfExtents[ 1 ] = aabb.len_y() / 2.f;
   mHalfExtents[ 2 ] = aabb.len_z() / 2.f;

   _initPoints();
}

void BoxConvex::getFeatures(const MatrixF& mat,const VectorF& n, ConvexFeature* cf)
{
   cf->material = 0;
   cf->object = mObject;

   S32 v = 0;
   v += (n.x >= 0)? 1: 0;
   v += (n.y >= 0)? 2: 0;
   v += (n.z >= 0)? 4: 0;

   PlaneF plane;
   plane.set(getVertex(v),n);

   // Emit vertex and edge
   S32 mask = 0;
   Corner& corner = sCorner[v];
   mask |= isOnPlane(getVertex(corner.a),plane)? 1: 0;
   mask |= isOnPlane(getVertex(corner.b),plane)? 2: 0;
   mask |= isOnPlane(getVertex(corner.c),plane)? 4: 0;

   switch(mask) {
      case 0: {
         cf->mVertexList.increment();
         mat.mulP(getVertex(v),&cf->mVertexList.last());
         break;
      }
      case 1:
         emitEdge(v,corner.a,mat,cf);
         break;
      case 2:
         emitEdge(v,corner.b,mat,cf);
         break;
      case 4:
         emitEdge(v,corner.c,mat,cf);
         break;
      case 1 | 2:
         emitFace(corner.ab,mat,cf);
         break;
      case 2 | 4:
         emitFace(corner.bc,mat,cf);
         break;
      case 1 | 4:
         emitFace(corner.ac,mat,cf);
         break;
   }
}