本文整理汇总了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;未经允许,请勿转载。 |
请发表评论