• 设为首页
  • 点击收藏
  • 手机版
    手机扫一扫访问
    迪恩网络手机版
  • 关注官方公众号
    微信扫一扫关注
    公众号
登陆 注册

C++ dgMatrix类代码示例

原作者: [db:作者] 来自: [db:来源] 收藏 邀请

本文整理汇总了C++中dgMatrix类的典型用法代码示例。如果您正苦于以下问题：C++ dgMatrix类的具体用法？C++ dgMatrix怎么用？C++ dgMatrix使用的例子？那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。



在下文中一共展示了dgMatrix类的20个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于我们的系统推荐出更棒的C++代码示例。

示例1: origin

void dgCollisionScene::CalcAABB (const dgMatrix& matrix, dgVector& p0, dgVector& p1) const
{
	dgVector origin (matrix.TransformVector(m_boxOrigin));
	dgVector size (m_boxSize.m_x * dgAbsf(matrix[0][0]) + m_boxSize.m_y * dgAbsf(matrix[1][0]) + m_boxSize.m_z * dgAbsf(matrix[2][0]) + DG_MAX_COLLISION_PADDING,  
				   m_boxSize.m_x * dgAbsf(matrix[0][1]) + m_boxSize.m_y * dgAbsf(matrix[1][1]) + m_boxSize.m_z * dgAbsf(matrix[2][1]) + DG_MAX_COLLISION_PADDING,  
				   m_boxSize.m_x * dgAbsf(matrix[0][2]) + m_boxSize.m_y * dgAbsf(matrix[1][2]) + m_boxSize.m_z * dgAbsf(matrix[2][2]) + DG_MAX_COLLISION_PADDING,
				   dgFloat32 (0.0f));

	p0 = origin - size;
	p1 = origin + size;

#ifdef DG_DEBUG_AABB
	dgInt32 i;
	dgVector q0;
	dgVector q1;
	dgMatrix trans (matrix.Transpose());
	for (i = 0; i < 3; i ++) {
		q0[i] = matrix.m_posit[i] + matrix.RotateVector (BoxSupportMapping(trans[i].Scale (-1.0f)))[i];
		q1[i] = matrix.m_posit[i] + matrix.RotateVector (BoxSupportMapping(trans[i]))[i];
	}

	dgVector err0 (p0 - q0);
	dgVector err1 (p1 - q1);
	dgFloat32 err; 
	err = GetMax (size.m_x, size.m_y, size.m_z) * 0.5f; 
	_ASSERTE ((err0 % err0) < err);
	_ASSERTE ((err1 % err1) < err);
#endif
}
开发者ID:mmozeiko，项目名称:Squares3D-Android，代码行数:29，代码来源:dgCollisionScene.cpp


示例2: OOBBTest

bool dgCollisionConvexHull::OOBBTest (const dgMatrix& matrix, const dgCollisionConvex* const shape, void* const cacheOrder) const
{
	bool ret;
	_ASSERTE (cacheOrder);

	ret = dgCollisionConvex::OOBBTest (matrix, shape, cacheOrder);
	if (ret) {
		const dgConvexSimplexEdge* const* faceArray = m_faceArray;
		dgCollisionBoundPlaneCache* const cache = (dgCollisionBoundPlaneCache*)cacheOrder;

		for (dgInt32 i = 0; i < dgInt32 (sizeof (cache->m_planes) / sizeof (dgPlane)); i ++) {
			dgFloat32 dist;
			const dgPlane& plane = cache->m_planes[i];
			if ((plane % plane) > dgFloat32 (0.0f)) {
				dgVector dir (matrix.UnrotateVector(plane.Scale (-1.0f)));
				dir.m_w = dgFloat32 (0.0f);
				dgVector p (matrix.TransformVector (shape->SupportVertex(dir)));
				dist = plane.Evalue (p);
				if (dist > dgFloat32 (0.1f)){
					return false;
				} 
			}
		}

		for (dgInt32 i = 0; i < m_boundPlanesCount; i ++) {
			dgInt32 i0;
			dgInt32 i1;
			dgInt32 i2;
			dgFloat32 dist;

			const dgConvexSimplexEdge* const face = faceArray[i];
			i0 = face->m_prev->m_vertex;
			i1 = face->m_vertex;
			i2 = face->m_next->m_vertex;
			const dgVector& p0 = m_vertex[i0];

			dgVector normal ((m_vertex[i1] - p0) * (m_vertex[i2] - p0));
			normal = normal.Scale (dgFloat32 (1.0f) / dgSqrt (normal % normal));

			dgVector dir (matrix.UnrotateVector(normal.Scale (-1.0f)));
			dir.m_w = dgFloat32 (0.0f);
			dgVector p (matrix.TransformVector (shape->SupportVertex(dir)));

			//_ASSERTE ((normal % (m_boxOrigin - p0)) < 0.0f);
			dist = normal % (p - p0);
			if (dist > dgFloat32 (0.1f)){
				for (dgInt32 j = 0; j < (dgInt32 (sizeof (cache->m_planes) / sizeof (dgPlane)) - 1); j ++) {
					cache->m_planes[j + 1] = cache->m_planes[j];
				}
				cache->m_planes[1] = dgPlane (normal, - (normal % p0));
				return false;
			} 
		}
	}
	return ret;
}
开发者ID:blabz2007，项目名称:prime.mod，代码行数:56，代码来源:dgCollisionConvexHull.cpp


示例3: BoundingBox

	// Compute axis aligned box
	static void BoundingBox (const dgMatrix &Mat, const hacd::HaF32 vertex[], hacd::HaI32 vertexCount, hacd::HaI32 stride, dgVector &min, dgVector &max)
	{
		hacd::HaF32 xmin = hacd::HaF32 (1.0e10f);
		hacd::HaF32 ymin = hacd::HaF32 (1.0e10f);
		hacd::HaF32 zmin = hacd::HaF32 (1.0e10f);

		hacd::HaF32 xmax = hacd::HaF32 (-1.0e10f);
		hacd::HaF32 ymax = hacd::HaF32 (-1.0e10f);
		hacd::HaF32 zmax = hacd::HaF32 (-1.0e10f);

		const hacd::HaF32* ptr = vertex;
		for (hacd::HaI32 i = 0 ; i < vertexCount; i ++ ) {
			dgVector tmp (ptr[0], ptr[1], ptr[2], hacd::HaF32 (0.0f));
			ptr += stride;
			tmp = Mat.UnrotateVector (tmp);
			if (tmp.m_x < xmin) xmin = tmp.m_x;
			if (tmp.m_y < ymin) ymin = tmp.m_y;
			if (tmp.m_z < zmin) zmin = tmp.m_z;
			if (tmp.m_x > xmax) xmax = tmp.m_x;
			if (tmp.m_y > ymax) ymax = tmp.m_y;
			if (tmp.m_z > zmax) zmax = tmp.m_z;
		}

		min = dgVector (xmin, ymin, zmin, hacd::HaF32 (0.0f));
		max = dgVector (xmax, ymax, zmax, hacd::HaF32 (0.0f));
	}
开发者ID:Reticulatas，项目名称:MochaEngineFinal，代码行数:27，代码来源:dgSphere.cpp


示例4: ConvexCast

dgInt32 dgBroadPhaseDefault::ConvexCast(dgCollisionInstance* const shape, const dgMatrix& matrix, const dgVector& target, dgFloat32& timeToImpact, OnRayPrecastAction prefilter, void* const userData, dgConvexCastReturnInfo* const info, dgInt32 maxContacts, dgInt32 threadIndex) const
{
	dgInt32 totalCount = 0;
	if (m_rootNode) {
		dgVector boxP0;
		dgVector boxP1;
		dgAssert(matrix.TestOrthogonal());
		shape->CalcAABB(matrix, boxP0, boxP1);

		dgFloat32 distance[DG_BROADPHASE_MAX_STACK_DEPTH];
		const dgBroadPhaseNode* stackPool[DG_BROADPHASE_MAX_STACK_DEPTH];

		dgVector velocA((target - matrix.m_posit) & dgVector::m_triplexMask);
		dgVector velocB(dgFloat32(0.0f));
		dgFastRayTest ray(dgVector(dgFloat32(0.0f)), velocA);

		dgVector minBox(m_rootNode->m_minBox - boxP1);
		dgVector maxBox(m_rootNode->m_maxBox - boxP0);
		stackPool[0] = m_rootNode;
		distance[0] = ray.BoxIntersect(minBox, maxBox);

		totalCount = dgBroadPhase::ConvexCast(stackPool, distance, 1, velocA, velocB, ray, shape, matrix, target, timeToImpact, prefilter, userData, info, maxContacts, threadIndex);
	}

	return totalCount;
}
开发者ID:leegoonz，项目名称:newton-dynamics，代码行数:26，代码来源:dgBroadPhaseDefault.cpp


示例5: CalcAABB

void dgCollisionDeformableMesh::CalcAABB (const dgMatrix& matrix, dgVector& p0, dgVector& p1) const
{
    dgVector origin (matrix.TransformVector(m_boxOrigin));
    dgVector size (matrix.m_front.Abs().Scale4(m_boxSize.m_x) + matrix.m_up.Abs().Scale4(m_boxSize.m_y) + matrix.m_right.Abs().Scale4(m_boxSize.m_z));

    p0 = (origin - size) & dgVector::m_triplexMask;
    p1 = (origin + size) & dgVector::m_triplexMask;
}
开发者ID:Kaoswerk，项目名称:newton-dynamics，代码行数:8，代码来源:dgCollisionDeformableMesh.cpp


示例6: dgAssert

void dgCollisionDeformableSolidMesh::SetMatrix(const dgMatrix& matrix)
{
   	dgAssert (m_myBody);
    //dgMatrix globalMatrix (GetBody()->GetCollision()->GetLocalMatrix() * matrix);
    for (dgInt32 i = 0; i < m_particles.m_count; i ++) {
        m_particles.m_posit[i] = m_shapePosit[i];
        m_posit[i] = matrix.TransformVector(m_shapePosit[i]) & dgVector::m_triplexMask;
    }
}
开发者ID:alexsaen，项目名称:newton-dynamics，代码行数:9，代码来源:dgCollisionDeformableSolidMesh.cpp


示例7: dgFloat32

void dgCollisionChamferCylinder::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
//dgCollisionConvex::DebugCollision (matrix, callback, userData);
//return;

	dgInt32 slices = 12;
	dgInt32 brakes = 24;
	dgFloat32 sliceAngle = dgFloat32 (0.0f);
	dgFloat32 sliceStep = dgPI  / slices; 
	dgFloat32 breakStep = dgPI2 / brakes;

	dgTriplex pool[24 * (12 + 1)];

	dgMatrix rot (dgPitchMatrix (breakStep));	
	dgInt32 index = 0;
	for (dgInt32 j = 0; j <= slices; j ++) {
		dgVector p0 (-m_height * dgCos(sliceAngle), dgFloat32 (0.0f), m_radius + m_height * dgSin(sliceAngle), dgFloat32 (0.0f));
		sliceAngle += sliceStep;
		for (dgInt32 i = 0; i < brakes; i ++) {
			pool[index].m_x = p0.m_x;
			pool[index].m_y = p0.m_y;
			pool[index].m_z = p0.m_z;
			index ++;
			p0 = rot.UnrotateVector (p0);
		}
	}

	//dgMatrix matrix (GetLocalMatrix() * matrixPtr);
	matrix.TransformTriplex (&pool[0].m_x, sizeof (dgTriplex), &pool[0].m_x, sizeof (dgTriplex), 24 * (12 + 1));

	dgTriplex face[32];

	index = 0;
	for (dgInt32 j = 0; j < slices; j ++) {
		dgInt32 index0 = index + brakes - 1;
		for (dgInt32 i = 0; i < brakes; i ++) {
			face[0] = pool[index];
			face[1] = pool[index0];
			face[2] = pool[index0 + brakes];
			face[3] = pool[index + brakes];
			index0 = index;
			index ++;
			callback (userData, 4, &face[0].m_x, 0);
		}
	}

	for (dgInt32 i = 0; i < brakes; i ++) { 
		face[i] = pool[i];
	}
	callback (userData, 24, &face[0].m_x, 0);

	for (dgInt32 i = 0; i < brakes; i ++) { 
		face[i] = pool[brakes * (slices + 1) - i - 1];
	}
	callback (userData, 24, &face[0].m_x, 0);
}
开发者ID:Hurleyworks，项目名称:MiniNewton，代码行数:56，代码来源:dgCollisionChamferCylinder.cpp


示例8: 

dgMatrix::dgMatrix (const dgMatrix& transformMatrix, const dgVector& scale, const dgMatrix& stretchAxis)
{
	dgMatrix scaledAxis;
	scaledAxis[0] = stretchAxis[0].Scale4 (scale[0]);
	scaledAxis[1] = stretchAxis[1].Scale4 (scale[1]);
	scaledAxis[2] = stretchAxis[2].Scale4 (scale[2]);
	scaledAxis[3] = stretchAxis[3];

	*this = stretchAxis.Transpose() * scaledAxis * transformMatrix;
}
开发者ID:Kaoswerk，项目名称:newton-dynamics，代码行数:10，代码来源:dgMatrix.cpp


示例9: dgSqrt

dgQuaternion::dgQuaternion (const dgMatrix& matrix)
{
	enum QUAT_INDEX
	{
		X_INDEX=0,
		Y_INDEX=1,
		Z_INDEX=2
	};
	static QUAT_INDEX QIndex [] = {Y_INDEX, Z_INDEX, X_INDEX};

	dgFloat32 trace = matrix[0][0] + matrix[1][1] + matrix[2][2];
	if (trace > dgFloat32(0.0f)) {
		trace = dgSqrt (trace + dgFloat32(1.0f));
		m_q0 = dgFloat32 (0.5f) * trace;
		trace = dgFloat32 (0.5f) / trace;
		m_q1 = (matrix[1][2] - matrix[2][1]) * trace;
		m_q2 = (matrix[2][0] - matrix[0][2]) * trace;
		m_q3 = (matrix[0][1] - matrix[1][0]) * trace;

	} else {
		QUAT_INDEX i = X_INDEX;
		if (matrix[Y_INDEX][Y_INDEX] > matrix[X_INDEX][X_INDEX]) {
			i = Y_INDEX;
		}
		if (matrix[Z_INDEX][Z_INDEX] > matrix[i][i]) {
			i = Z_INDEX;
		}
		QUAT_INDEX j = QIndex [i];
		QUAT_INDEX k = QIndex [j];

		trace = dgFloat32(1.0f) + matrix[i][i] - matrix[j][j] - matrix[k][k];
		trace = dgSqrt (trace);

		dgFloat32* const ptr = &m_q1;
		ptr[i] = dgFloat32 (0.5f) * trace;
		trace  = dgFloat32 (0.5f) / trace;
		m_q0   = (matrix[j][k] - matrix[k][j]) * trace;
		ptr[j] = (matrix[i][j] + matrix[j][i]) * trace;
		ptr[k] = (matrix[i][k] + matrix[k][i]) * trace;
	}

#ifdef _DEBUG
	dgMatrix tmp (*this, matrix.m_posit);
	dgMatrix unitMatrix (tmp * matrix.Inverse());
	for (dgInt32 i = 0; i < 4; i ++) {
		dgFloat32 err = dgAbsf (unitMatrix[i][i] - dgFloat32(1.0f));
		dgAssert (err < dgFloat32 (1.0e-2f));
	}

	dgFloat32 err = dgAbsf (DotProduct(*this) - dgFloat32(1.0f));
	dgAssert (err < dgFloat32(dgEPSILON * 100.0f));
#endif
}
开发者ID:Hurleyworks，项目名称:MiniNewton，代码行数:53，代码来源:dgQuaternion.cpp


示例10: DebugCollision

void dgCollisionDeformableMesh::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
	const dgVector* const particlePosit = m_particles.m_posit;
	for (dgInt32 i = 0; i < m_trianglesCount; i ++ ) {
		dgTriplex points[3];
		for (dgInt32 j = 0; j < 3; j ++) {
			dgInt32 index = m_indexList[i * 3 + j];
			dgVector p (matrix.TransformVector(particlePosit[index]));
			points[j].m_x = p.m_x;
			points[j].m_y = p.m_y;
			points[j].m_z = p.m_z;
		}
		callback (userData, 3, &points[0].m_x, 0);
	}
}
开发者ID:Kaoswerk，项目名称:newton-dynamics，代码行数:15，代码来源:dgCollisionDeformableMesh.cpp


示例11: BoundingBox

	static void BoundingBox (
		const dgMatrix &matrix,
		const dgFloat32 vertex[], 
		dgInt32 stride, 
		const dgInt32 index[],
		dgInt32 indexCount, 
		dgVector &min, 
		dgVector &max)
	{
		dgInt32 i;
		dgInt32 j;
		const dgFloat32 *ptr;
		dgFloat32 xmin;
		dgFloat32 xmax;
		dgFloat32 ymin;
		dgFloat32 ymax;
		dgFloat32 zmin;
		dgFloat32 zmax;
		
	   xmin = dgFloat32 (1.0e10f);
	   ymin = dgFloat32 (1.0e10f);
	   zmin = dgFloat32 (1.0e10f);

	   xmax = dgFloat32 (-1.0e10f);
	   ymax = dgFloat32 (-1.0e10f);
	   zmax = dgFloat32 (-1.0e10f);


		ptr = vertex;
	   for (j = 0 ; j < indexCount; j ++ ) {
			i = index[j] * stride;
			dgVector tmp (ptr[i + 0], ptr[i + 1], ptr[i + 2], dgFloat32 (0.0f));
			tmp = matrix.UnrotateVector (tmp);
			if (tmp.m_x < xmin) xmin = tmp.m_x;
			if (tmp.m_y < ymin) ymin = tmp.m_y;
			if (tmp.m_z < zmin) zmin = tmp.m_z;
			if (tmp.m_x > xmax) xmax = tmp.m_x;
			if (tmp.m_y > ymax) ymax = tmp.m_y;
			if (tmp.m_z > zmax) zmax = tmp.m_z;
	   }
		
	   min = dgVector (xmin, ymin, zmin, dgFloat32 (0.0f));
	   max = dgVector (xmax, ymax, zmax, dgFloat32 (0.0f));
	}
开发者ID:mmozeiko，项目名称:Squares3D-Android，代码行数:44，代码来源:dgSphere.cpp


示例12: DebugCollision

void dgCollisionTaperedCylinder::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
	dgTriplex pool[24 * 2];

	dgFloat32 angle = dgFloat32 (0.0f);
	for (dgInt32 i = 0; i < 24; i ++) {
		dgFloat32 z = dgSin (angle);
		dgFloat32 y = dgCos (angle);
		pool[i].m_x = - m_height;
		pool[i].m_y = y * m_radio1;
		pool[i].m_z = z * m_radio1;
		pool[i + 24].m_x = m_height;
		pool[i + 24].m_y = y * m_radio0;
		pool[i + 24].m_z = z * m_radio0;
		angle += dgPI2 / dgFloat32 (24.0f);
	}

	matrix.TransformTriplex (&pool[0].m_x, sizeof (dgTriplex), &pool[0].m_x, sizeof (dgTriplex), 24 * 2);

	dgTriplex face[24];

	dgInt32 j = 24 - 1;
	for (dgInt32 i = 0; i < 24; i ++) { 
		face[0] = pool[j];
		face[1] = pool[i];
		face[2] = pool[i + 24];
		face[3] = pool[j + 24];
		j = i;
		callback (userData, 4, &face[0].m_x, 0);
	}

	for (dgInt32 i = 0; i < 24; i ++) { 
		face[i] = pool[24 - 1 - i];
	}
	callback (userData, 24, &face[0].m_x, 0);
		
	for (dgInt32 i = 0; i < 24; i ++) { 
		face[i] = pool[i + 24];
	}
	callback (userData, 24, &face[0].m_x, 0);
}
开发者ID:famorcia，项目名称:newton-dynamics，代码行数:41，代码来源:dgCollisionTaperedCylinder.cpp


示例13: dgAssert

dgVector dgBilateralConstraint::CalculateGlobalMatrixAndAngle (dgMatrix& globalMatrix0, dgMatrix& globalMatrix1) const
{
	dgAssert (m_body0);
	dgAssert (m_body1);
	const dgMatrix& body0Matrix = m_body0->GetMatrix();
	const dgMatrix& body1Matrix = m_body1->GetMatrix();

	globalMatrix0 = m_localMatrix0 * body0Matrix;
	globalMatrix1 = m_localMatrix1 * body1Matrix;

	dgMatrix relMatrix (globalMatrix1 * globalMatrix0.Inverse());

	dgAssert (dgAbsf (dgFloat32 (1.0f) - (relMatrix.m_front % relMatrix.m_front)) < 1.0e-5f); 
	dgAssert (dgAbsf (dgFloat32 (1.0f) - (relMatrix.m_up % relMatrix.m_up)) < 1.0e-5f); 
	dgAssert (dgAbsf (dgFloat32 (1.0f) - (relMatrix.m_right % relMatrix.m_right)) < 1.0e-5f); 

	dgVector euler0;
	dgVector euler1;
	relMatrix.CalcPitchYawRoll (euler0, euler1);
	return euler0;
}
开发者ID:souxiaosou，项目名称:newton-dynamics，代码行数:21，代码来源:dgBilateralConstraint.cpp


示例14: DebugCollision

void dgCollisionCone::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
//dgCollisionConvex::DebugCollision (matrix, callback, userData);
//return;

	#define NUMBER_OF_DEBUG_SEGMENTS  40
	dgTriplex pool[NUMBER_OF_DEBUG_SEGMENTS + 1];
	dgTriplex face[NUMBER_OF_DEBUG_SEGMENTS];

	dgFloat32 angle = dgFloat32 (0.0f);
	for (dgInt32 i = 0; i < NUMBER_OF_DEBUG_SEGMENTS; i ++) {
		dgFloat32 z = dgSin (angle) * m_radius;
		dgFloat32 y = dgCos (angle) * m_radius;
		pool[i].m_x = -m_height;
		pool[i].m_y = y;
		pool[i].m_z = z;
		angle += dgPI2 / dgFloat32 (NUMBER_OF_DEBUG_SEGMENTS);
	}
	
	pool[NUMBER_OF_DEBUG_SEGMENTS].m_x = m_height;
	pool[NUMBER_OF_DEBUG_SEGMENTS].m_y = dgFloat32 (0.0f);
	pool[NUMBER_OF_DEBUG_SEGMENTS].m_z = dgFloat32 (0.0f);

	matrix.TransformTriplex (&pool[0].m_x, sizeof (dgTriplex), &pool[0].m_x, sizeof (dgTriplex), NUMBER_OF_DEBUG_SEGMENTS + 1);
	dgInt32 j = NUMBER_OF_DEBUG_SEGMENTS - 1;
	for (dgInt32 i = 0; i < NUMBER_OF_DEBUG_SEGMENTS; i ++) { 
		face[0] = pool[j];
		face[1] = pool[i];
		face[2] = pool[NUMBER_OF_DEBUG_SEGMENTS];
		j = i;
		callback (userData, 3, &face[0].m_x, 0);
	}

	for (dgInt32 i = 0; i < NUMBER_OF_DEBUG_SEGMENTS; i ++) { 
		face[i] = pool[NUMBER_OF_DEBUG_SEGMENTS - 1 - i];
	}
	callback (userData, NUMBER_OF_DEBUG_SEGMENTS, &face[0].m_x, 0);
}
开发者ID:Hurleyworks，项目名称:MiniNewton，代码行数:38，代码来源:dgCollisionCone.cpp


示例15: DebugCollision

void dgCollisionConvexHull::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
//	dgTriplex tmp[1024 * 4];
//	matrix.TransformTriplex (&tmp[0].m_x, sizeof (dgTriplex), &m_vertex[0].m_x, sizeof (dgVector), m_vertexCount);

	dgTriplex vertex[256];
	for (dgInt32 i = 0; i < m_faceCount; i ++) {
		dgConvexSimplexEdge* const face = m_faceArray[i];
		dgConvexSimplexEdge* ptr = face;
		dgInt32 count = 0;
		do {
			//vertex[count] = tmp[ptr->m_vertex];
			vertex[count].m_x = m_vertex[ptr->m_vertex].m_x;
			vertex[count].m_y = m_vertex[ptr->m_vertex].m_y;
			vertex[count].m_z = m_vertex[ptr->m_vertex].m_z;
			count ++;
			dgAssert (count < sizeof (vertex)/ sizeof (vertex[0]));
			ptr = ptr->m_next;
		} while (ptr != face);
		matrix.TransformTriplex (&vertex[0].m_x, sizeof (dgTriplex), &vertex[0].m_x, sizeof (dgTriplex), count);
		callback (userData, count, &vertex[0].m_x, 0);
	}
}
开发者ID:Hurleyworks，项目名称:MiniNewton，代码行数:23，代码来源:dgCollisionConvexHull.cpp


示例16: _ASSERTE

dgVector dgBilateralConstraint::CalculateGlobalMatrixAndAngle (dgMatrix& globalMatrix0, dgMatrix& globalMatrix1) const
{
	_ASSERTE (m_body0);
	_ASSERTE (m_body1);
	const dgMatrix& body0Matrix = m_body0->GetMatrix();
	const dgMatrix& body1Matrix = m_body1->GetMatrix();
//	dgMatrix body1Matrix (dgGetIdentityMatrix());
//	if (m_body1) {
//		body1Matrix = m_body1->GetMatrix();
//	}

	globalMatrix0 = m_localMatrix0 * body0Matrix;
	globalMatrix1 = m_localMatrix1 * body1Matrix;

	dgMatrix relMatrix (globalMatrix1 * globalMatrix0.Inverse());

	_ASSERTE (dgAbsf (dgFloat32 (1.0f) - (relMatrix.m_front % relMatrix.m_front)) < 1.0e-5f); 
	_ASSERTE (dgAbsf (dgFloat32 (1.0f) - (relMatrix.m_up % relMatrix.m_up)) < 1.0e-5f); 
	_ASSERTE (dgAbsf (dgFloat32 (1.0f) - (relMatrix.m_right % relMatrix.m_right)) < 1.0e-5f); 
//	_ASSERTE ((relMatrix.m_posit % relMatrix.m_posit) < 1.0e-3f);

	return relMatrix.CalcPitchYawRoll ();
}
开发者ID:Naddiseo，项目名称:Newton-Dynamics-fork，代码行数:23，代码来源:dgBilateralConstraint.cpp


示例17: DebugCollision

void dgCollisionTaperedCapsule::DebugCollision (const dgMatrix& matrix, dgCollision::OnDebugCollisionMeshCallback callback, void* const userData) const
{
	#define POWER 2
	dgVector tmpVectex[1024 * 2];

	dgVector p0 ( dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); 
	dgVector p1 (-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); 
	dgVector p2 ( dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)); 
	dgVector p3 ( dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
	dgVector p4 ( dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f));
	dgVector p5 ( dgFloat32 (0.0f), dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f));

	dgInt32 count = 0;
	TesselateTriangle (POWER, p0, p2, p4, count, tmpVectex);
	TesselateTriangle (POWER, p0, p4, p3, count, tmpVectex);
	TesselateTriangle (POWER, p0, p3, p5, count, tmpVectex);
	TesselateTriangle (POWER, p0, p5, p2, count, tmpVectex);

	TesselateTriangle (POWER, p1, p4, p2, count, tmpVectex);
	TesselateTriangle (POWER, p1, p3, p4, count, tmpVectex);
	TesselateTriangle (POWER, p1, p5, p3, count, tmpVectex);
	TesselateTriangle (POWER, p1, p2, p5, count, tmpVectex);

	dgFloat32 scale = m_radio1 / m_radio0;
	dgVector edgeP0(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
	dgVector edgeP1(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));

	for (dgInt32 i = 0; i < count; i += 3) {
		dgVector face0[4]; 	
		dgVector face1[4]; 	
		dgInt32 n0 = 0;
		dgInt32 n1 = 0;
		dgVector p0 (tmpVectex[i + 2]);
		for (dgInt32 j = 0; j < 3; j ++) {
			dgVector p1 (tmpVectex[i + j]);
			if (p1.m_x > m_clip0) {
				if (p0.m_x < m_clip0) {
					dgFloat32 t = (m_clip0 - p0.m_x) / (p1.m_x - p0.m_x);
					edgeP0 = p0 + (p1 - p0).Scale3 (t);
					edgeP0.m_x = m_clip0;

					face0[n0] = edgeP0;
					face0[n0].m_x += m_height;
					n0 ++;

					face1[n1] = edgeP0.Scale3 (scale);
					face1[n1].m_x -= m_height;
					n1 ++;
				}
				face0[n0] = p1;
				face0[n0].m_x += m_height;
				n0 ++;
			} else {
				if (p0.m_x > m_clip0) {
					dgFloat32 t = (m_clip0 - p0.m_x) / (p1.m_x - p0.m_x);
					edgeP1 = p0 + (p1 - p0).Scale3 (t);
					edgeP1.m_x = m_clip0;

					face0[n0] = edgeP1;
					face0[n0].m_x += m_height;
					n0 ++;

					face1[n1] = edgeP1.Scale3 (scale);
					face1[n1].m_x -= m_height;
					n1 ++;
				}
				face1[n1] = p1.Scale3 (scale);;
				face1[n1].m_x -= m_height;
				
				n1 ++;
			}
			p0 = p1;
		}

		dgTriplex face[4];
		if (n0) {
			matrix.TransformTriplex (&face[0].m_x, sizeof (dgTriplex), &face0[0].m_x, sizeof (dgVector), n0);			
			callback (userData, n0, &face[0].m_x, 0);
		}
		if (n1) {
			matrix.TransformTriplex (&face[0].m_x, sizeof (dgTriplex), &face1[0].m_x, sizeof (dgVector), n1);
			callback (userData, n1, &face[0].m_x, 0);
		}
		if (n0 && n1) {
			face0[0] = edgeP0;
			face0[1] = edgeP1;
			face0[2] = edgeP1.Scale3 (scale);
			face0[3] = edgeP0.Scale3 (scale);
			face0[0].m_x += m_height;
			face0[1].m_x += m_height;
			face0[2].m_x -= m_height;
			face0[3].m_x -= m_height;

			dgPlane plane (face0[0], face0[1], face0[2]);
			if (plane.m_w >= dgFloat32 (0.0f)) {
				dgAssert (0);
			}

			matrix.TransformTriplex (&face[0].m_x, sizeof (dgTriplex), &face0[0].m_x, sizeof (dgVector), 4);
			callback (userData, 4, &face[0].m_x, 0);
//.........这里部分代码省略.........
开发者ID:famorcia，项目名称:newton-dynamics，代码行数:101，代码来源:dgCollisionTaperedCapsule.cpp


示例18: AddMesh

void dgPolygonSoupDatabaseBuilder::AddMesh (const hacd::HaF32* const vertex, hacd::HaI32 vertexCount, hacd::HaI32 strideInBytes, hacd::HaI32 faceCount,	
	const hacd::HaI32* const faceArray, const hacd::HaI32* const indexArray, const hacd::HaI32* const faceTagsData, const dgMatrix& worldMatrix) 
{
	hacd::HaI32 faces[256];
	hacd::HaI32 pool[2048];


	m_vertexPoints[m_vertexCount + vertexCount].m_x = hacd::HaF64 (0.0f);
	dgBigVector* const vertexPool = &m_vertexPoints[m_vertexCount];

	worldMatrix.TransformTriplex (&vertexPool[0].m_x, sizeof (dgBigVector), vertex, strideInBytes, vertexCount);
	for (hacd::HaI32 i = 0; i < vertexCount; i ++) {
		vertexPool[i].m_w = hacd::HaF64 (0.0f);
	}

	hacd::HaI32 totalIndexCount = faceCount;
	for (hacd::HaI32 i = 0; i < faceCount; i ++) {
		totalIndexCount += faceArray[i];
	}

	m_vertexIndex[m_indexCount + totalIndexCount] = 0;
	m_faceVertexCount[m_faceCount + faceCount] = 0;

	hacd::HaI32 k = 0;
	for (hacd::HaI32 i = 0; i < faceCount; i ++) {
		hacd::HaI32 count = faceArray[i];
		for (hacd::HaI32 j = 0; j < count; j ++) {
			hacd::HaI32 index = indexArray[k];
			pool[j] = index + m_vertexCount;
			k ++;
		}

		hacd::HaI32 convexFaces = 0;
		if (count == 3) {
			convexFaces = 1;
			dgBigVector p0 (m_vertexPoints[pool[2]]);
			for (hacd::HaI32 i = 0; i < 3; i ++) {
				dgBigVector p1 (m_vertexPoints[pool[i]]);
				dgBigVector edge (p1 - p0);
				hacd::HaF64 mag2 = edge % edge;
				if (mag2 < hacd::HaF32 (1.0e-6f)) {
					convexFaces = 0;
				}
				p0 = p1;
			}

			if (convexFaces) {
				dgBigVector edge0 (m_vertexPoints[pool[2]] - m_vertexPoints[pool[0]]);
				dgBigVector edge1 (m_vertexPoints[pool[1]] - m_vertexPoints[pool[0]]);
				dgBigVector normal (edge0 * edge1);
				hacd::HaF64 mag2 = normal % normal;
				if (mag2 < hacd::HaF32 (1.0e-8f)) {
					convexFaces = 0;
				}
			}

			if (convexFaces) {
				faces[0] = 3;
			}

		} else {
			convexFaces = AddConvexFace (count, pool, faces);
		}

		hacd::HaI32 index = 0;
		for (hacd::HaI32 k = 0; k < convexFaces; k ++) {
			hacd::HaI32 count = faces[k];
			m_vertexIndex[m_indexCount] = faceTagsData[i];
			m_indexCount ++;
			for (hacd::HaI32 j = 0; j < count; j ++) {
				m_vertexIndex[m_indexCount] = pool[index];
				index ++;
				m_indexCount ++;
			}
			m_faceVertexCount[m_faceCount] = count + 1;
			m_faceCount ++;
		}
	}
	m_vertexCount += vertexCount;
	m_run -= vertexCount;
	if (m_run <= 0) {
		PackArray();
	}
}
开发者ID:andrewkaufman，项目名称:hacd，代码行数:84，代码来源:dgPolygonSoupBuilder.cpp


示例19: BuildJacobianMatrix

DG_INLINE void dgSolver::BuildJacobianMatrix(dgJointInfo* const jointInfo, dgLeftHandSide* const leftHandSide, dgRightHandSide* const rightHandSide)
{
	const dgInt32 m0 = jointInfo->m_m0;
	const dgInt32 m1 = jointInfo->m_m1;
	const dgInt32 index = jointInfo->m_pairStart;
	const dgInt32 count = jointInfo->m_pairCount;
	const dgDynamicBody* const body0 = (dgDynamicBody*)m_bodyArray[m0].m_body;
	const dgDynamicBody* const body1 = (dgDynamicBody*)m_bodyArray[m1].m_body;
	const bool isBilateral = jointInfo->m_joint->IsBilateral();

	const dgMatrix invInertia0 = body0->m_invWorldInertiaMatrix;
	const dgMatrix invInertia1 = body1->m_invWorldInertiaMatrix;
	const dgVector invMass0(body0->m_invMass[3]);
	const dgVector invMass1(body1->m_invMass[3]);

	dgSoaFloat force0(m_soaZero);
	if (body0->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
		force0 = dgSoaFloat(body0->m_externalForce, body0->m_externalTorque);
	}

	dgSoaFloat force1(m_soaZero);
	if (body1->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
		force1 = dgSoaFloat(body1->m_externalForce, body1->m_externalTorque);
	}

	jointInfo->m_preconditioner0 = dgFloat32(1.0f);
	jointInfo->m_preconditioner1 = dgFloat32(1.0f);
	if ((invMass0.GetScalar() > dgFloat32(0.0f)) && (invMass1.GetScalar() > dgFloat32(0.0f)) && !(body0->GetSkeleton() && body1->GetSkeleton())) {
		const dgFloat32 mass0 = body0->GetMass().m_w;
		const dgFloat32 mass1 = body1->GetMass().m_w;
		if (mass0 > (DG_DIAGONAL_PRECONDITIONER * mass1)) {
			jointInfo->m_preconditioner0 = mass0 / (mass1 * DG_DIAGONAL_PRECONDITIONER);
		} else if (mass1 > (DG_DIAGONAL_PRECONDITIONER * mass0)) {
			jointInfo->m_preconditioner1 = mass1 / (mass0 * DG_DIAGONAL_PRECONDITIONER);
		}
	}

	const dgFloat32 forceImpulseScale = dgFloat32(1.0f);
	const dgSoaFloat weight0(m_bodyProxyArray[m0].m_weight * jointInfo->m_preconditioner0);
	const dgSoaFloat weight1(m_bodyProxyArray[m1].m_weight * jointInfo->m_preconditioner0);
	for (dgInt32 i = 0; i < count; i++) {
		dgLeftHandSide* const row = &leftHandSide[index + i];
		dgRightHandSide* const rhs = &rightHandSide[index + i];

		row->m_JMinv.m_jacobianM0.m_linear = row->m_Jt.m_jacobianM0.m_linear * invMass0;
		row->m_JMinv.m_jacobianM0.m_angular = invInertia0.RotateVector(row->m_Jt.m_jacobianM0.m_angular);
		row->m_JMinv.m_jacobianM1.m_linear = row->m_Jt.m_jacobianM1.m_linear * invMass1;
		row->m_JMinv.m_jacobianM1.m_angular = invInertia1.RotateVector(row->m_Jt.m_jacobianM1.m_angular);

		const dgSoaFloat& JMinvM0 = (dgSoaFloat&)row->m_JMinv.m_jacobianM0;
		const dgSoaFloat& JMinvM1 = (dgSoaFloat&)row->m_JMinv.m_jacobianM1;
		const dgSoaFloat tmpAccel((JMinvM0 * force0).MulAdd(JMinvM1, force1));

		dgFloat32 extenalAcceleration = -tmpAccel.AddHorizontal();
		rhs->m_deltaAccel = extenalAcceleration * forceImpulseScale;
		rhs->m_coordenateAccel += extenalAcceleration * forceImpulseScale;
		dgAssert(rhs->m_jointFeebackForce);
		const dgFloat32 force = rhs->m_jointFeebackForce->m_force * forceImpulseScale;
		rhs->m_force = isBilateral ? dgClamp(force, rhs->m_lowerBoundFrictionCoefficent, rhs->m_upperBoundFrictionCoefficent) : force;
		rhs->m_maxImpact = dgFloat32(0.0f);

		const dgSoaFloat& JtM0 = (dgSoaFloat&)row->m_Jt.m_jacobianM0;
		const dgSoaFloat& JtM1 = (dgSoaFloat&)row->m_Jt.m_jacobianM1;
		const dgSoaFloat tmpDiag((weight0 * JMinvM0 * JtM0).MulAdd(weight1, JMinvM1 * JtM1));

		dgFloat32 diag = tmpDiag.AddHorizontal();
		dgAssert(diag > dgFloat32(0.0f));
		rhs->m_diagDamp = diag * rhs->m_stiffness;
		diag *= (dgFloat32(1.0f) + rhs->m_stiffness);
		//rhs->m_jinvMJt = diag;
		rhs->m_invJinvMJt = dgFloat32(1.0f) / diag;
	}
}
开发者ID:Hurleyworks，项目名称:newton-dynamics，代码行数:73，代码来源:dgSolver.cpp


示例20: eigenVectors

void dgMatrix::EigenVectors (dgVector &eigenValues, const dgMatrix& initialGuess)
{
	hacd::HaF32 b[3];
	hacd::HaF32 z[3];
	hacd::HaF32 d[3];

	dgMatrix& mat = *this;
	dgMatrix eigenVectors (initialGuess.Transpose4X4());
	mat = initialGuess * mat * eigenVectors;

	b[0] = mat[0][0]; 
	b[1] = mat[1][1];
	b[2] = mat[2][2];

	d[0] = mat[0][0]; 
	d[1] = mat[1][1]; 
	d[2] = mat[2][2]; 

	z[0] = hacd::HaF32 (0.0f);
	z[1] = hacd::HaF32 (0.0f);
	z[2] = hacd::HaF32 (0.0f);

	for (hacd::HaI32 i = 0; i < 50; i++) {
		hacd::HaF32 sm = dgAbsf(mat[0][1]) + dgAbsf(mat[0][2]) + dgAbsf(mat[1][2]);

		if (sm < hacd::HaF32 (1.0e-6f)) {
			HACD_ASSERT (dgAbsf((eigenVectors.m_front % eigenVectors.m_front) - hacd::HaF32(1.0f)) < dgEPSILON);
			HACD_ASSERT (dgAbsf((eigenVectors.m_up % eigenVectors.m_up) - hacd::HaF32(1.0f)) < dgEPSILON);
			HACD_ASSERT (dgAbsf((eigenVectors.m_right % eigenVectors.m_right) - hacd::HaF32(1.0f)) < dgEPSILON);

			// order the eigenvalue vectors	
			dgVector tmp (eigenVectors.m_front * eigenVectors.m_up);
			if (tmp % eigenVectors.m_right < hacd::HaF32(0.0f)) {
				eigenVectors.m_right = eigenVectors.m_right.Scale (-hacd::HaF32(1.0f));
			}

			eigenValues = dgVector (d[0], d[1], d[2], hacd::HaF32 (0.0f));
			*this = eigenVectors.Inverse();
			return;
		}

		hacd::HaF32 thresh = hacd::HaF32 (0.0f);
		if (i < 3) {
			thresh = (hacd::HaF32)(0.2f / 9.0f) * sm;
		}

		for (hacd::HaI32 ip = 0; ip < 2; ip ++) {
			for (hacd::HaI32 iq = ip + 1; iq < 3; iq ++) {
				hacd::HaF32 g = hacd::HaF32 (100.0f) * dgAbsf(mat[ip][iq]);
				//if ((i > 3) && (dgAbsf(d[0]) + g == dgAbsf(d[ip])) && (dgAbsf(d[1]) + g == dgAbsf(d[1]))) {
				if ((i > 3) && ((dgAbsf(d[ip]) + g) == dgAbsf(d[ip])) && ((dgAbsf(d[iq]) + g) == dgAbsf(d[iq]))) {
					mat[ip][iq] = hacd::HaF32 (0.0f);
				} else if (dgAbsf(mat[ip][iq]) > thresh) {

					hacd::HaF32 t;
					hacd::HaF32 h = d[iq] - d[ip];
					if (dgAbsf(h) + g == dgAbsf(h)) {
						t = mat[ip][iq] / h;
					} else {
						hacd::HaF32 theta = hacd::HaF32 (0.5f) * h / mat[ip][iq];
						t = hacd::HaF32(1.0f) / (dgAbsf(theta) + dgSqrt(hacd::HaF32(1.0f) + theta * theta));
						if (theta < hacd::HaF32 (0.0f)) {
							t = -t;
						}
					}
					hacd::HaF32 c = hacd::HaF32(1.0f) / dgSqrt (hacd::HaF32 (1.0f) + t * t); 
					hacd::HaF32 s = t * c; 
					hacd::HaF32 tau = s / (hacd::HaF32(1.0f) + c); 
					h = t * mat[ip][iq];
					z[ip] -= h; 
					z[iq] += h; 
					d[ip] -= h; 
					d[iq] += h;
					mat[ip][iq] = hacd::HaF32(0.0f);

					for (hacd::HaI32 j = 0; j <= ip - 1; j ++) {
						//ROT (mat, j, ip, j, iq, s, tau); 
						//ROT(dgMatrix &a, hacd::HaI32 i, hacd::HaI32 j, hacd::HaI32 k, hacd::HaI32 l, hacd::HaF32 s, hacd::HaF32 tau) 
						hacd::HaF32 g = mat[j][ip]; 
						hacd::HaF32 h = mat[j][iq]; 
						mat[j][ip] = g - s * (h + g * tau); 
						mat[j][iq] = h + s * (g - h * tau);

					}
					for (hacd::HaI32 j = ip + 1; j <= iq - 1; j ++) {
						//ROT (mat, ip, j, j, iq, s, tau); 
						//ROT(dgMatrix &a, hacd::HaI32 i, hacd::HaI32 j, hacd::HaI32 k, hacd::HaI32 l, hacd::HaF32 s, hacd::HaF32 tau) 
						hacd::HaF32 g = mat[ip][j]; 
						hacd::HaF32 h = mat[j][iq]; 
						mat[ip][j] = g - s * (h + g * tau); 
						mat[j][iq] = h + s * (g - h * tau);
					}
					for (hacd::HaI32 j = iq + 1; j < 3; j ++) {
						//ROT (mat, ip, j, iq, j, s, tau); 
						//ROT(dgMatrix &a, hacd::HaI32 i, hacd::HaI32 j, hacd::HaI32 k, hacd::HaI32 l, hacd::HaF32 s, hacd::HaF32 tau) 
						hacd::HaF32 g = mat[ip][j]; 
						hacd::HaF32 h = mat[iq][j]; 
						mat[ip][j] = g - s * (h + g * tau); 
						mat[iq][j] = h + s * (g - h * tau);
					}
//.........这里部分代码省略.........
开发者ID:qimiaokeju11，项目名称:testhacd，代码行数:101，代码来源:dgMatrix.cpp



注：本文中的dgMatrix类示例由纯净天空整理自Github/MSDocs等源码及文档管理平台，相关代码片段筛选自各路编程大神贡献的开源项目，源码版权归原作者所有，传播和使用请参考对应项目的License；未经允许，请勿转载。

鲜花
握手
雷人
路过
鸡蛋
该文章已有0人参与评论

请发表评论

全部评论

专题导读
More+
上一篇：
C++ dgVector类代码示例发布时间：2022-05-31
下一篇：
C++ dfu_bank_func_t类代码示例发布时间：2022-05-31
热门推荐
More+
热门话题
More+
阅读排行榜

扫描微信二维码

查看手机版网站

随时了解更新最新资讯

139-2527-9053

在线客服（服务时间 9:00～18:00）

在线QQ客服
地址：深圳市南山区西丽大学城创智工业园
电邮：jeky_zhao#qq.com
移动电话：139-2527-9053

Powered by 互联科技 X3.4© 2001-2213 极客世界.|Sitemap