本文整理汇总了C++中dgVector::AddHorizontal方法的典型用法代码示例。如果您正苦于以下问题：C++ dgVector::AddHorizontal方法的具体用法？C++ dgVector::AddHorizontal怎么用？C++ dgVector::AddHorizontal使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类dgVector的用法示例。
在下文中一共展示了dgVector::AddHorizontal方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: BuildJacobianMatrix
DG_INLINE void dgSolver::BuildJacobianMatrix(dgJointInfo* const jointInfo, dgLeftHandSide* const leftHandSide, dgRightHandSide* const rightHandSide, dgJacobian* const internalForces)
{
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]);
dgVector force0(m_zero);
dgVector torque0(m_zero);
if (body0->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
force0 = body0->m_externalForce;
torque0 = body0->m_externalTorque;
}
dgVector force1(m_zero);
dgVector torque1(m_zero);
if (body1->IsRTTIType(dgBody::m_dynamicBodyRTTI)) {
force1 = body1->m_externalForce;
torque1 = 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);
}
}
//dgSoaFloat forceAcc0(m_soaZero);
//dgSoaFloat forceAcc1(m_soaZero);
dgVector forceAcc0(m_zero);
dgVector torqueAcc0(m_zero);
dgVector forceAcc1(m_zero);
dgVector torqueAcc1(m_zero);
//const dgSoaFloat weight0(m_bodyProxyArray[m0].m_weight * jointInfo->m_preconditioner0);
//const dgSoaFloat weight1(m_bodyProxyArray[m1].m_weight * jointInfo->m_preconditioner0);
const dgVector weight0(m_bodyProxyArray[m0].m_weight * jointInfo->m_preconditioner0);
const dgVector weight1(m_bodyProxyArray[m1].m_weight * jointInfo->m_preconditioner0);
const dgFloat32 forceImpulseScale = dgFloat32(1.0f);
const dgFloat32 preconditioner0 = jointInfo->m_preconditioner0;
const dgFloat32 preconditioner1 = jointInfo->m_preconditioner1;
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));
const dgJacobian& JMinvM0 = row->m_JMinv.m_jacobianM0;
const dgJacobian& JMinvM1 = row->m_JMinv.m_jacobianM1;
const dgVector tmpAccel(JMinvM0.m_linear * force0 + JMinvM0.m_angular * torque0 +
JMinvM1.m_linear * force1 + JMinvM1.m_angular * torque1);
dgFloat32 extenalAcceleration = -tmpAccel.AddHorizontal().GetScalar();
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));
const dgJacobian& JtM0 = row->m_Jt.m_jacobianM0;
const dgJacobian& JtM1 = row->m_Jt.m_jacobianM1;
const dgVector tmpDiag(weight0 * (JMinvM0.m_linear * JtM0.m_linear + JMinvM0.m_angular * JtM0.m_angular) +
weight1 * (JMinvM1.m_linear * JtM1.m_linear + JMinvM1.m_angular * JtM1.m_angular));
dgFloat32 diag = tmpDiag.AddHorizontal().GetScalar();
dgAssert(diag > dgFloat32(0.0f));
rhs->m_diagDamp = diag * rhs->m_stiffness;
diag *= (dgFloat32(1.0f) + rhs->m_stiffness);
rhs->m_invJinvMJt = dgFloat32(1.0f) / diag;
dgVector f0(rhs->m_force * preconditioner0);
dgVector f1(rhs->m_force * preconditioner1);
//forceAcc0 = forceAcc0.MulAdd(JtM0, f0);
//forceAcc1 = forceAcc1.MulAdd(JtM1, f1);
//.........这里部分代码省略.........