protected void InternalUpdateSprings(ConstraintInfo2 info) { // it is assumed that calculateTransforms() have been called before this call IndexedVector3 relVel = m_rbB.GetLinearVelocity() - m_rbA.GetLinearVelocity(); for (int i = 0; i < 3; i++) { if (m_springEnabled[i]) { // get current position of constraint float currPos = m_calculatedLinearDiff[i]; // calculate difference float delta = currPos - m_equilibriumPoint[i]; // spring force is (delta * m_stiffness) according to Hooke's Law float force = delta * m_springStiffness[i]; float velFactor = info.fps * m_springDamping[i] / (float)info.m_numIterations; m_linearLimits.m_targetVelocity[i] = velFactor * force; m_linearLimits.m_maxMotorForce[i] = Math.Abs(force) / info.fps; } } for (int i = 0; i < 3; i++) { if (m_springEnabled[i + 3]) { // get current position of constraint float currPos = m_calculatedAxisAngleDiff[i]; // calculate difference float delta = currPos - m_equilibriumPoint[i + 3]; // spring force is (-delta * m_stiffness) according to Hooke's Law float force = -delta * m_springStiffness[i + 3]; float velFactor = info.fps * m_springDamping[i + 3] / (float)info.m_numIterations; m_angularLimits[i].m_targetVelocity = velFactor * force; m_angularLimits[i].m_maxMotorForce = Math.Abs(force) / info.fps; } } }
protected virtual int SetAngularLimits(ConstraintInfo2 info, int row_offset, ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedVector3 linVelA, ref IndexedVector3 linVelB, ref IndexedVector3 angVelA, ref IndexedVector3 angVelB) { Generic6DofConstraint d6constraint = this; int row = row_offset; //solve angular limits for (int i = 0; i < 3; i++) { if (d6constraint.GetRotationalLimitMotor(i).NeedApplyTorques()) { IndexedVector3 axis = d6constraint.GetAxis(i); int tempFlags = ((int)m_flags) >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT); SixDofFlags flags = (SixDofFlags)tempFlags; if (0 == (flags & SixDofFlags.BT_6DOF_FLAGS_CFM_NORM)) { m_angularLimits[i].m_normalCFM = info.m_solverConstraints[0].m_cfm; } if (0 == (flags & SixDofFlags.BT_6DOF_FLAGS_CFM_STOP)) { m_angularLimits[i].m_stopCFM = info.m_solverConstraints[0].m_cfm; } if (0 == (flags & SixDofFlags.BT_6DOF_FLAGS_ERP_STOP)) { m_angularLimits[i].m_stopERP = info.erp; } row += GetLimitMotorInfo2(d6constraint.GetRotationalLimitMotor(i), ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB, info, row, ref axis, 1, false); } } return row; }
public override void GetInfo2(ConstraintInfo2 info) { if (m_useOffsetForConstraintFrame) { GetInfo2InternalUsingFrameOffset(info, m_rbA.GetCenterOfMassTransform(), m_rbB.GetCenterOfMassTransform(), m_rbA.GetAngularVelocity(), m_rbB.GetAngularVelocity()); } else { GetInfo2Internal(info, m_rbA.GetCenterOfMassTransform(), m_rbB.GetCenterOfMassTransform(), m_rbA.GetAngularVelocity(), m_rbB.GetAngularVelocity()); } }
protected void InternalUpdateSprings(ConstraintInfo2 info) { // it is assumed that calculateTransforms() have been called before this call IndexedVector3 relVel = m_rbB.GetLinearVelocity() - m_rbA.GetLinearVelocity(); for (int i = 0; i < 3; i++) { if (m_springEnabled[i]) { // get current position of constraint float currPos = m_calculatedLinearDiff[i]; // calculate difference float delta = currPos - m_equilibriumPoint[i]; // spring force is (delta * m_stiffness) according to Hooke's Law float force = delta * m_springStiffness[i]; float velFactor = info.fps * m_springDamping[i] / (float)info.m_numIterations; m_linearLimits.m_targetVelocity[i] = velFactor * force; m_linearLimits.m_maxMotorForce[i] = Math.Abs(force) / info.fps; } } for (int i = 0; i < 3; i++) { if (m_springEnabled[i + 3]) { // get current position of constraint float currPos = m_calculatedAxisAngleDiff[i]; // calculate difference float delta = currPos - m_equilibriumPoint[i + 3]; // spring force is (-delta * m_stiffness) according to Hooke's Law float force = -delta * m_springStiffness[i + 3]; float velFactor = info.fps * m_springDamping[i + 3] / (float)info.m_numIterations; m_angularLimits[i].m_targetVelocity = velFactor * force; m_angularLimits[i].m_maxMotorForce = Math.Abs(force) / info.fps; } } }
public virtual void GetInfo2(ConstraintInfo2 info) { }
public virtual void GetInfo2(ConstraintInfo2 info) { }
public override void GetInfo2(ConstraintInfo2 info) { GetInfo2NonVirtual(info, m_rbA.GetCenterOfMassTransform(), m_rbB.GetCenterOfMassTransform(), m_rbA.GetLinearVelocity(), m_rbB.GetLinearVelocity(), m_rbA.GetInvMass(), m_rbB.GetInvMass()); }
public void TestLinLimits2(ConstraintInfo2 info) { }
public override void GetInfo2(ConstraintInfo2 info) { GetInfo2NonVirtual(info, m_rbA.GetCenterOfMassTransform(), m_rbB.GetCenterOfMassTransform(), m_rbA.GetLinearVelocity(), m_rbB.GetLinearVelocity(), m_rbA.GetInvMass(), m_rbB.GetInvMass()); }
public void GetInfo2InternalUsingFrameOffset(ConstraintInfo2 info, IndexedMatrix transA, IndexedMatrix transB, IndexedVector3 angVelA, IndexedVector3 angVelB) { GetInfo2InternalUsingFrameOffset(info, ref transA, ref transB, ref angVelA, ref angVelB); }
public override void GetInfo2(ConstraintInfo2 info) { }
public void GetInfo2NonVirtual(ConstraintInfo2 info, IndexedMatrix body0_trans, IndexedMatrix body1_trans) { // anchor points in global coordinates with respect to body PORs. // set jacobian info.m_solverConstraints[0].m_contactNormal.X = 1; info.m_solverConstraints[1].m_contactNormal.Y = 1; info.m_solverConstraints[2].m_contactNormal.Z = 1; IndexedVector3 a1 = body0_trans._basis * GetPivotInA(); { IndexedVector3 a1neg = -a1; MathUtil.GetSkewSymmetricMatrix(ref a1neg, out info.m_solverConstraints[0].m_relpos1CrossNormal, out info.m_solverConstraints[1].m_relpos1CrossNormal, out info.m_solverConstraints[2].m_relpos1CrossNormal); } /*info->m_J2linearAxis[0] = -1; * info->m_J2linearAxis[s+1] = -1; * info->m_J2linearAxis[2*s+2] = -1; */ IndexedVector3 a2 = body1_trans._basis * GetPivotInB(); { IndexedVector3 a2n = -a2; MathUtil.GetSkewSymmetricMatrix(ref a2, out info.m_solverConstraints[0].m_relpos2CrossNormal, out info.m_solverConstraints[1].m_relpos2CrossNormal, out info.m_solverConstraints[2].m_relpos2CrossNormal); } // set right hand side float currERP = ((m_flags & Point2PointFlags.BT_P2P_FLAGS_ERP) != 0) ? m_erp : info.erp; float k = info.fps * currERP; int j; IndexedVector3 body0Origin = body0_trans._origin; IndexedVector3 body1Origin = body1_trans._origin; for (j = 0; j < 3; j++) { info.m_solverConstraints[j].m_rhs = k * (a2[j] + body1Origin[j] - a1[j] - body0Origin[j]); //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); } if ((m_flags & Point2PointFlags.BT_P2P_FLAGS_CFM) != 0) { for (j = 0; j < 3; j++) { info.m_solverConstraints[j].m_cfm = m_cfm; } } float impulseClamp = m_setting.m_impulseClamp;// for (j = 0; j < 3; j++) { if (m_setting.m_impulseClamp > 0) { info.m_solverConstraints[j].m_lowerLimit = -impulseClamp; info.m_solverConstraints[j].m_upperLimit = impulseClamp; } } info.m_damping = m_setting.m_damping; }
public void GetInfo2NonVirtual(ConstraintInfo2 info, IndexedMatrix transA, IndexedMatrix transB, IndexedVector3 angVelA, IndexedVector3 angVelB) { ///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now TestLimit(ref transA, ref transB); GetInfo2Internal(info, transA, transB, angVelA, angVelB); }
public void GetInfo2NonVirtual(ConstraintInfo2 info,IndexedMatrix body0_trans,IndexedMatrix body1_trans) { // anchor points in global coordinates with respect to body PORs. // set jacobian info.m_solverConstraints[0].m_contactNormal.X = 1; info.m_solverConstraints[1].m_contactNormal.Y = 1; info.m_solverConstraints[2].m_contactNormal.Z = 1; IndexedVector3 a1 = body0_trans._basis * GetPivotInA(); { IndexedVector3 a1neg = -a1; MathUtil.GetSkewSymmetricMatrix(ref a1neg, out info.m_solverConstraints[0].m_relpos1CrossNormal, out info.m_solverConstraints[1].m_relpos1CrossNormal, out info.m_solverConstraints[2].m_relpos1CrossNormal); } /*info->m_J2linearAxis[0] = -1; info->m_J2linearAxis[s+1] = -1; info->m_J2linearAxis[2*s+2] = -1; */ IndexedVector3 a2 = body1_trans._basis * GetPivotInB(); { IndexedVector3 a2n = -a2; MathUtil.GetSkewSymmetricMatrix(ref a2, out info.m_solverConstraints[0].m_relpos2CrossNormal, out info.m_solverConstraints[1].m_relpos2CrossNormal, out info.m_solverConstraints[2].m_relpos2CrossNormal); } // set right hand side float currERP = ((m_flags & Point2PointFlags.BT_P2P_FLAGS_ERP) != 0) ? m_erp : info.erp; float k = info.fps * currERP; int j; IndexedVector3 body0Origin = body0_trans._origin; IndexedVector3 body1Origin = body1_trans._origin; for (j = 0; j < 3; j++) { info.m_solverConstraints[j].m_rhs = k * (a2[j] + body1Origin[j] - a1[j] - body0Origin[j]); //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); } if ((m_flags & Point2PointFlags.BT_P2P_FLAGS_CFM) != 0) { for (j = 0; j < 3; j++) { info.m_solverConstraints[j].m_cfm = m_cfm; } } float impulseClamp = m_setting.m_impulseClamp;// for (j = 0; j < 3; j++) { if (m_setting.m_impulseClamp > 0) { info.m_solverConstraints[j].m_lowerLimit = -impulseClamp; info.m_solverConstraints[j].m_upperLimit = impulseClamp; } } info.m_damping = m_setting.m_damping; }
public void GetInfo2NonVirtual(ConstraintInfo2 info, IndexedMatrix transA, IndexedMatrix transB, IndexedBasisMatrix invInertiaWorldA, IndexedBasisMatrix invInertiaWorldB) { CalcAngleInfo2(ref transA, ref transB, ref invInertiaWorldA, ref invInertiaWorldB); Debug.Assert(!m_useSolveConstraintObsolete); // set jacobian info.m_solverConstraints[0].m_contactNormal.X = 1f; info.m_solverConstraints[1].m_contactNormal.Y = 1f; info.m_solverConstraints[2].m_contactNormal.Z = 1f; IndexedVector3 a1 = transA._basis * m_rbAFrame._origin; { IndexedVector3 a1neg = -a1; MathUtil.GetSkewSymmetricMatrix(ref a1neg, out info.m_solverConstraints[0].m_relpos1CrossNormal, out info.m_solverConstraints[1].m_relpos1CrossNormal, out info.m_solverConstraints[2].m_relpos1CrossNormal); } IndexedVector3 a2 = transB._basis * m_rbBFrame._origin; { MathUtil.GetSkewSymmetricMatrix(ref a2, out info.m_solverConstraints[0].m_relpos2CrossNormal, out info.m_solverConstraints[1].m_relpos2CrossNormal, out info.m_solverConstraints[2].m_relpos2CrossNormal); } // set right hand side float linERP = ((m_flags & (int)ConeTwistFlags.BT_CONETWIST_FLAGS_LIN_ERP) != 0) ? m_linERP : info.erp; float k = info.fps * linERP; for (int j = 0; j < 3; j++) { info.m_solverConstraints[j].m_rhs = k * (a2[j] + transB._origin[j] - a1[j] - transA._origin[j]); info.m_solverConstraints[j].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[j].m_upperLimit = MathUtil.SIMD_INFINITY; if ((m_flags & (int)ConeTwistFlags.BT_CONETWIST_FLAGS_LIN_CFM) != 0) { info.m_solverConstraints[j].m_cfm = m_linCFM; } } int row = 3; IndexedVector3 ax1; // angular limits if (m_solveSwingLimit) { if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) { IndexedMatrix trA = transA * m_rbAFrame; IndexedVector3 p = trA._basis.GetColumn(1); IndexedVector3 q = trA._basis.GetColumn(2); info.m_solverConstraints[row].m_relpos1CrossNormal = p; info.m_solverConstraints[row + 1].m_relpos1CrossNormal = q; info.m_solverConstraints[row].m_relpos2CrossNormal = -p; info.m_solverConstraints[row + 1].m_relpos2CrossNormal = -q; float fact = info.fps * m_relaxationFactor; info.m_solverConstraints[row].m_rhs = fact * m_swingAxis.Dot(ref p); info.m_solverConstraints[row + 1].m_rhs = fact * m_swingAxis.Dot(ref q); info.m_solverConstraints[row].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[row].m_upperLimit = MathUtil.SIMD_INFINITY; info.m_solverConstraints[row + 1].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[row + 1].m_upperLimit = MathUtil.SIMD_INFINITY; row += 2; } else { ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor; info.m_solverConstraints[row].m_relpos1CrossNormal = ax1; info.m_solverConstraints[row].m_relpos2CrossNormal = -ax1; float k1 = info.fps * m_biasFactor; info.m_solverConstraints[row].m_rhs = k1 * m_swingCorrection; if ((m_flags & (int)ConeTwistFlags.BT_CONETWIST_FLAGS_ANG_CFM) != 0) { info.m_solverConstraints[row].m_cfm = m_angCFM; } // m_swingCorrection is always positive or 0 info.m_solverConstraints[row].m_lowerLimit = 0; info.m_solverConstraints[row].m_upperLimit = MathUtil.SIMD_INFINITY; ++row; } } if (m_solveTwistLimit) { ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor; info.m_solverConstraints[row].m_relpos1CrossNormal = ax1; info.m_solverConstraints[row].m_relpos2CrossNormal = -ax1; float k1 = info.fps * m_biasFactor; info.m_solverConstraints[row].m_rhs = k1 * m_twistCorrection; if ((m_flags & (int)ConeTwistFlags.BT_CONETWIST_FLAGS_ANG_CFM) != 0) { info.m_solverConstraints[row].m_cfm = m_angCFM; } if (m_twistSpan > 0.0f) { if (m_twistCorrection > 0.0f) { info.m_solverConstraints[row].m_lowerLimit = 0; info.m_solverConstraints[row].m_upperLimit = MathUtil.SIMD_INFINITY; } else { info.m_solverConstraints[row].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[row].m_upperLimit = 0; } } else { info.m_solverConstraints[row].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[row].m_upperLimit = MathUtil.SIMD_INFINITY; } ++row; } if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints) { //PrintInfo2(BulletGlobals.g_streamWriter, this, info); } }
public override void GetInfo2(ConstraintInfo2 info) { GetInfo2NonVirtual(info, m_rbA.GetCenterOfMassTransform(), m_rbB.GetCenterOfMassTransform(), m_rbA.GetInvInertiaTensorWorld(), m_rbB.GetInvInertiaTensorWorld()); }
public void GetInfo2NonVirtual(ConstraintInfo2 info, IndexedMatrix transA, IndexedMatrix transB, IndexedVector3 angVelA, IndexedVector3 angVelB) { ///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now TestLimit(ref transA, ref transB); GetInfo2Internal(info, transA, transB, angVelA, angVelB); }
public void GetInfo2Internal(ConstraintInfo2 info, IndexedMatrix transA, IndexedMatrix transB, IndexedVector3 angVelA, IndexedVector3 angVelB) { // transforms in world space IndexedMatrix trA = transA * m_rbAFrame; IndexedMatrix trB = transB * m_rbBFrame; // pivot point IndexedVector3 pivotAInW = trA._origin; IndexedVector3 pivotBInW = trB._origin; // linear (all fixed) //info.m_J1linearAxis[0] = 1; //info.m_J1linearAxis[s + 1] = 1; //info.m_J1linearAxis[2 * s + 2] = 1; if (!m_angularOnly) { info.m_solverConstraints[0].m_contactNormal.X = 1f; info.m_solverConstraints[1].m_contactNormal.Y = 1f; info.m_solverConstraints[2].m_contactNormal.Z = 1f; } IndexedVector3 a1 = pivotAInW - transA._origin; { IndexedVector3 a1neg = -a1; MathUtil.GetSkewSymmetricMatrix(ref a1neg, out info.m_solverConstraints[0].m_relpos1CrossNormal, out info.m_solverConstraints[1].m_relpos1CrossNormal, out info.m_solverConstraints[2].m_relpos1CrossNormal); //if (info.m_solverConstraints[0].m_relpos1CrossNormal.X == 0.15) //{ // int ibreak = 0; //} int ibreak = 0; } IndexedVector3 a2 = pivotBInW - transB._origin; { MathUtil.GetSkewSymmetricMatrix(ref a2, out info.m_solverConstraints[0].m_relpos2CrossNormal, out info.m_solverConstraints[1].m_relpos2CrossNormal, out info.m_solverConstraints[2].m_relpos2CrossNormal); } // linear RHS float k = info.fps * info.erp; if (!m_angularOnly) { for (int i = 0; i < 3; i++) { float val = k * (pivotBInW[i] - pivotAInW[i]); info.m_solverConstraints[i].m_rhs = val; } } // make rotations around X and Y equal // the hinge axis should be the only unconstrained // rotational axis, the angular velocity of the two bodies perpendicular to // the hinge axis should be equal. thus the constraint equations are // p*w1 - p*w2 = 0 // q*w1 - q*w2 = 0 // where p and q are unit vectors normal to the hinge axis, and w1 and w2 // are the angular velocity vectors of the two bodies. // get hinge axis (Z) IndexedVector3 ax1 = trA._basis.GetColumn(2); // get 2 orthos to hinge axis (X, Y) IndexedVector3 p = trA._basis.GetColumn(0); IndexedVector3 q = trA._basis.GetColumn(1); // set the two hinge angular rows MathUtil.SanityCheckVector(ax1); MathUtil.SanityCheckVector(p); MathUtil.SanityCheckVector(q); int s3 = 3; int s4 = 4; info.m_solverConstraints[s3].m_relpos1CrossNormal = p; info.m_solverConstraints[s4].m_relpos1CrossNormal = q; info.m_solverConstraints[s3].m_relpos2CrossNormal = -p; info.m_solverConstraints[s4].m_relpos2CrossNormal = -q; // compute the right hand side of the constraint equation. set relative // body velocities along p and q to bring the hinge back into alignment. // if ax1,ax2 are the unit length hinge axes as computed from body1 and // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). // if `theta' is the angle between ax1 and ax2, we need an angular velocity // along u to cover angle erp*theta in one step : // |angular_velocity| = angle/time = erp*theta / stepsize // = (erp*fps) * theta // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) // ...as ax1 and ax2 are unit length. if theta is smallish, // theta ~= sin(theta), so // angular_velocity = (erp*fps) * (ax1 x ax2) // ax1 x ax2 is in the plane space of ax1, so we project the angular // velocity to p and q to find the right hand side. IndexedVector3 ax2 = trB._basis.GetColumn(2); IndexedVector3 u = IndexedVector3.Cross(ax1, ax2); info.m_solverConstraints[s3].m_rhs = k * IndexedVector3.Dot(u, p); info.m_solverConstraints[s4].m_rhs = k * IndexedVector3.Dot(u, q); // check angular limits int nrow = 4; // last filled row float limit_err = 0.0f; int limit = 0; if (GetSolveLimit()) { #if _BT_USE_CENTER_LIMIT_ limit_err = m_limit.GetCorrection() * m_referenceSign; #else limit_err = m_correction * m_referenceSign; #endif } // if the hinge has joint limits or motor, add in the extra row bool powered = false; if (GetEnableAngularMotor()) { powered = true; } if (limit != 0 || powered) { nrow++; info.m_solverConstraints[nrow].m_relpos1CrossNormal = ax1; info.m_solverConstraints[nrow].m_relpos2CrossNormal = -ax1; float lostop = GetLowerLimit(); float histop = GetUpperLimit(); if (limit != 0 && (MathUtil.CompareFloat(lostop, histop))) { // the joint motor is ineffective powered = false; } info.m_solverConstraints[nrow].m_rhs = 0.0f; float currERP = ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_ERP_STOP) != 0) ? m_stopERP : info.erp; if (powered) { if ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_CFM_NORM) != 0) { info.m_solverConstraints[nrow].m_cfm = m_normalCFM; } float mot_fact = GetMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info.fps * currERP); info.m_solverConstraints[nrow].m_rhs += mot_fact * m_motorTargetVelocity * m_referenceSign; info.m_solverConstraints[nrow].m_lowerLimit = -m_maxMotorImpulse; info.m_solverConstraints[nrow].m_upperLimit = m_maxMotorImpulse; } if (limit != 0) { k = info.fps * currERP; info.m_solverConstraints[nrow].m_rhs += k * limit_err; if ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_CFM_STOP) != 0) { info.m_solverConstraints[nrow].m_cfm = m_stopCFM; } if (MathUtil.CompareFloat(lostop, histop)) { // limited low and high simultaneously info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY; } else if (limit == 1) { // low limit info.m_solverConstraints[nrow].m_lowerLimit = 0f; info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY; } else { // high limit info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[nrow].m_upperLimit = 0f; } // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) #if _BT_USE_CENTER_LIMIT_ float bounce = m_limit.GetRelaxationFactor(); #else float bounce = m_relaxationFactor; #endif if (bounce > 0f) { float vel = IndexedVector3.Dot(angVelA, ax1); vel -= IndexedVector3.Dot(angVelB, ax1); // only apply bounce if the velocity is incoming, and if the // resulting c[] exceeds what we already have. if (limit == 1) { // low limit if (vel < 0) { float newc = -bounce * vel; if (newc > info.m_solverConstraints[nrow].m_rhs) { info.m_solverConstraints[nrow].m_rhs = newc; } } } else { // high limit - all those computations are reversed if (vel > 0) { float newc = -bounce * vel; if (newc < info.m_solverConstraints[nrow].m_rhs) { info.m_solverConstraints[nrow].m_rhs = newc; } } } } #if _BT_USE_CENTER_LIMIT_ info.m_solverConstraints[nrow].m_rhs *= m_limit.GetBiasFactor(); #else info.m_solverConstraints[nrow].m_rhs *= m_biasFactor; #endif } // if(limit) } // if angular limit or powered #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints) { PrintInfo2(BulletGlobals.g_streamWriter, this, info); } #endif }
public override void GetInfo2(ConstraintInfo2 info) { }
public override void GetInfo2(ConstraintInfo2 info) { if (m_useOffsetForConstraintFrame) { GetInfo2InternalUsingFrameOffset(info, m_rbA.GetCenterOfMassTransform(), m_rbB.GetCenterOfMassTransform(), m_rbA.GetAngularVelocity(), m_rbB.GetAngularVelocity()); } else { GetInfo2Internal(info, m_rbA.GetCenterOfMassTransform(), m_rbB.GetCenterOfMassTransform(), m_rbA.GetAngularVelocity(), m_rbB.GetAngularVelocity()); } }
public void GetInfo2InternalUsingFrameOffset(ConstraintInfo2 info, IndexedMatrix transA, IndexedMatrix transB, IndexedVector3 angVelA, IndexedVector3 angVelB) { GetInfo2InternalUsingFrameOffset(info, ref transA, ref transB, ref angVelA, ref angVelB); }
public void GetInfo2Internal(ConstraintInfo2 info, IndexedMatrix transA, IndexedMatrix transB, IndexedVector3 angVelA, IndexedVector3 angVelB) { // transforms in world space IndexedMatrix trA = transA * m_rbAFrame; IndexedMatrix trB = transB * m_rbBFrame; // pivot point IndexedVector3 pivotAInW = trA._origin; IndexedVector3 pivotBInW = trB._origin; // linear (all fixed) //info.m_J1linearAxis[0] = 1; //info.m_J1linearAxis[s + 1] = 1; //info.m_J1linearAxis[2 * s + 2] = 1; if (!m_angularOnly) { info.m_solverConstraints[0].m_contactNormal.X = 1f; info.m_solverConstraints[1].m_contactNormal.Y = 1f; info.m_solverConstraints[2].m_contactNormal.Z = 1f; } IndexedVector3 a1 = pivotAInW - transA._origin; { IndexedVector3 a1neg = -a1; MathUtil.GetSkewSymmetricMatrix(ref a1neg, out info.m_solverConstraints[0].m_relpos1CrossNormal, out info.m_solverConstraints[1].m_relpos1CrossNormal, out info.m_solverConstraints[2].m_relpos1CrossNormal); //if (info.m_solverConstraints[0].m_relpos1CrossNormal.X == 0.15) //{ // int ibreak = 0; //} int ibreak = 0; } IndexedVector3 a2 = pivotBInW - transB._origin; { MathUtil.GetSkewSymmetricMatrix(ref a2, out info.m_solverConstraints[0].m_relpos2CrossNormal, out info.m_solverConstraints[1].m_relpos2CrossNormal, out info.m_solverConstraints[2].m_relpos2CrossNormal); } // linear RHS float k = info.fps * info.erp; if (!m_angularOnly) { for (int i = 0; i < 3; i++) { float val = k * (pivotBInW[i] - pivotAInW[i]); info.m_solverConstraints[i].m_rhs = val; } } // make rotations around X and Y equal // the hinge axis should be the only unconstrained // rotational axis, the angular velocity of the two bodies perpendicular to // the hinge axis should be equal. thus the constraint equations are // p*w1 - p*w2 = 0 // q*w1 - q*w2 = 0 // where p and q are unit vectors normal to the hinge axis, and w1 and w2 // are the angular velocity vectors of the two bodies. // get hinge axis (Z) IndexedVector3 ax1 = trA._basis.GetColumn(2); // get 2 orthos to hinge axis (X, Y) IndexedVector3 p = trA._basis.GetColumn(0); IndexedVector3 q = trA._basis.GetColumn(1); // set the two hinge angular rows MathUtil.SanityCheckVector(ax1); MathUtil.SanityCheckVector(p); MathUtil.SanityCheckVector(q); int s3 = 3; int s4 = 4; info.m_solverConstraints[s3].m_relpos1CrossNormal = p; info.m_solverConstraints[s4].m_relpos1CrossNormal = q; info.m_solverConstraints[s3].m_relpos2CrossNormal = -p; info.m_solverConstraints[s4].m_relpos2CrossNormal = -q; // compute the right hand side of the constraint equation. set relative // body velocities along p and q to bring the hinge back into alignment. // if ax1,ax2 are the unit length hinge axes as computed from body1 and // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). // if `theta' is the angle between ax1 and ax2, we need an angular velocity // along u to cover angle erp*theta in one step : // |angular_velocity| = angle/time = erp*theta / stepsize // = (erp*fps) * theta // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) // ...as ax1 and ax2 are unit length. if theta is smallish, // theta ~= sin(theta), so // angular_velocity = (erp*fps) * (ax1 x ax2) // ax1 x ax2 is in the plane space of ax1, so we project the angular // velocity to p and q to find the right hand side. IndexedVector3 ax2 = trB._basis.GetColumn(2); IndexedVector3 u = IndexedVector3.Cross(ax1, ax2); info.m_solverConstraints[s3].m_rhs = k * IndexedVector3.Dot(u, p); info.m_solverConstraints[s4].m_rhs = k * IndexedVector3.Dot(u, q); // check angular limits int nrow = 4; // last filled row float limit_err = 0.0f; int limit = 0; if (GetSolveLimit()) { #if _BT_USE_CENTER_LIMIT_ limit_err = m_limit.GetCorrection() * m_referenceSign; #else limit_err = m_correction * m_referenceSign; #endif } // if the hinge has joint limits or motor, add in the extra row bool powered = false; if (GetEnableAngularMotor()) { powered = true; } if (limit != 0 || powered) { nrow++; info.m_solverConstraints[nrow].m_relpos1CrossNormal = ax1; info.m_solverConstraints[nrow].m_relpos2CrossNormal = -ax1; float lostop = GetLowerLimit(); float histop = GetUpperLimit(); if (limit != 0 && (MathUtil.CompareFloat(lostop, histop))) { // the joint motor is ineffective powered = false; } info.m_solverConstraints[nrow].m_rhs = 0.0f; float currERP = ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_ERP_STOP) != 0) ? m_stopERP : info.erp; if (powered) { if ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_CFM_NORM) != 0) { info.m_solverConstraints[nrow].m_cfm = m_normalCFM; } float mot_fact = GetMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info.fps * currERP); info.m_solverConstraints[nrow].m_rhs += mot_fact * m_motorTargetVelocity * m_referenceSign; info.m_solverConstraints[nrow].m_lowerLimit = -m_maxMotorImpulse; info.m_solverConstraints[nrow].m_upperLimit = m_maxMotorImpulse; } if (limit != 0) { k = info.fps * currERP; info.m_solverConstraints[nrow].m_rhs += k * limit_err; if ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_CFM_STOP) != 0) { info.m_solverConstraints[nrow].m_cfm = m_stopCFM; } if (MathUtil.CompareFloat(lostop, histop)) { // limited low and high simultaneously info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY; } else if (limit == 1) { // low limit info.m_solverConstraints[nrow].m_lowerLimit = 0f; info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY; } else { // high limit info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[nrow].m_upperLimit = 0f; } // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) #if _BT_USE_CENTER_LIMIT_ float bounce = m_limit.GetRelaxationFactor(); #else float bounce = m_relaxationFactor; #endif if (bounce > 0f) { float vel = IndexedVector3.Dot(angVelA, ax1); vel -= IndexedVector3.Dot(angVelB, ax1); // only apply bounce if the velocity is incoming, and if the // resulting c[] exceeds what we already have. if (limit == 1) { // low limit if (vel < 0) { float newc = -bounce * vel; if (newc > info.m_solverConstraints[nrow].m_rhs) { info.m_solverConstraints[nrow].m_rhs = newc; } } } else { // high limit - all those computations are reversed if (vel > 0) { float newc = -bounce * vel; if (newc < info.m_solverConstraints[nrow].m_rhs) { info.m_solverConstraints[nrow].m_rhs = newc; } } } } #if _BT_USE_CENTER_LIMIT_ info.m_solverConstraints[nrow].m_rhs *= m_limit.GetBiasFactor(); #else info.m_solverConstraints[nrow].m_rhs *= m_biasFactor; #endif } // if(limit) } // if angular limit or powered if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints) { PrintInfo2(BulletGlobals.g_streamWriter, this, info); } }
public void GetInfo2InternalUsingFrameOffset(ConstraintInfo2 info, ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedVector3 angVelA, ref IndexedVector3 angVelB) { // transforms in world space #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints) { MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "rbAFrame", m_rbAFrame); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "rbBFrame", m_rbBFrame); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transA", transA); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transB", transB); } #endif IndexedMatrix trA = transA * m_rbAFrame; IndexedMatrix trB = transB * m_rbBFrame; #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints) { MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "trA", trA); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "trB", trB); } #endif // pivot point IndexedVector3 pivotAInW = trA._origin; IndexedVector3 pivotBInW = trB._origin; #if true // difference between frames in WCS IndexedVector3 ofs = trB._origin - trA._origin; // now get weight factors depending on masses float miA = GetRigidBodyA().GetInvMass(); float miB = GetRigidBodyB().GetInvMass(); bool hasStaticBody = (miA < MathUtil.SIMD_EPSILON) || (miB < MathUtil.SIMD_EPSILON); float miS = miA + miB; float factA, factB; if (miS > 0.0f) { factA = miB / miS; } else { factA = 0.5f; } factB = 1.0f - factA; // get the desired direction of hinge axis // as weighted sum of Z-orthos of frameA and frameB in WCS IndexedVector3 ax1A = trA._basis.GetColumn(2); IndexedVector3 ax1B = trB._basis.GetColumn(2); IndexedVector3 ax1 = ax1A * factA + ax1B * factB; ax1.Normalize(); // fill first 3 rows // we want: velA + wA x relA == velB + wB x relB IndexedMatrix bodyA_trans = transA; IndexedMatrix bodyB_trans = transB; int s0 = 0; int s1 = 1; int s2 = 2; int nrow = 2; // last filled row IndexedVector3 tmpA, tmpB, relA, relB, p, q; // get vector from bodyB to frameB in WCS relB = trB._origin - bodyB_trans._origin; // get its projection to hinge axis IndexedVector3 projB = ax1 * IndexedVector3.Dot(relB, ax1); // get vector directed from bodyB to hinge axis (and orthogonal to it) IndexedVector3 orthoB = relB - projB; // same for bodyA relA = trA._origin - bodyA_trans._origin; IndexedVector3 projA = ax1 * IndexedVector3.Dot(relA, ax1); IndexedVector3 orthoA = relA - projA; IndexedVector3 totalDist = projA - projB; // get offset vectors relA and relB relA = orthoA + totalDist * factA; relB = orthoB - totalDist * factB; // now choose average ortho to hinge axis p = orthoB * factA + orthoA * factB; float len2 = p.LengthSquared(); if (len2 > MathUtil.SIMD_EPSILON) { p.Normalize(); } else { p = trA._basis.GetColumn(1); } // make one more ortho q = IndexedVector3.Cross(ax1, p); // fill three rows tmpA = IndexedVector3.Cross(relA, p); tmpB = IndexedVector3.Cross(relB, p); info.m_solverConstraints[s0].m_relpos1CrossNormal = tmpA; info.m_solverConstraints[s0].m_relpos2CrossNormal = -tmpB; tmpA = IndexedVector3.Cross(ref relA, ref q); tmpB = IndexedVector3.Cross(ref relB, ref q); if (hasStaticBody && GetSolveLimit()) { // to make constraint between static and dynamic objects more rigid // remove wA (or wB) from equation if angular limit is hit tmpB *= factB; tmpA *= factA; } info.m_solverConstraints[s1].m_relpos1CrossNormal = tmpA; info.m_solverConstraints[s1].m_relpos2CrossNormal = -tmpB; tmpA = IndexedVector3.Cross(ref relA, ref ax1); tmpB = IndexedVector3.Cross(ref relB, ref ax1); if (hasStaticBody) { // to make constraint between static and dynamic objects more rigid // remove wA (or wB) from equation tmpB *= factB; tmpA *= factA; } info.m_solverConstraints[s2].m_relpos1CrossNormal = tmpA; info.m_solverConstraints[s2].m_relpos2CrossNormal = -tmpB; float k = info.fps * info.erp; if (!m_angularOnly) { info.m_solverConstraints[s0].m_contactNormal = p; info.m_solverConstraints[s1].m_contactNormal = q; info.m_solverConstraints[s2].m_contactNormal = ax1; // compute three elements of right hand side float rhs = k * IndexedVector3.Dot(ref p, ref ofs); info.m_solverConstraints[s0].m_rhs = rhs; rhs = k * IndexedVector3.Dot(ref q, ref ofs); info.m_solverConstraints[s1].m_rhs = rhs; rhs = k * IndexedVector3.Dot(ref ax1, ref ofs); info.m_solverConstraints[s2].m_rhs = rhs; } // the hinge axis should be the only unconstrained // rotational axis, the angular velocity of the two bodies perpendicular to // the hinge axis should be equal. thus the constraint equations are // p*w1 - p*w2 = 0 // q*w1 - q*w2 = 0 // where p and q are unit vectors normal to the hinge axis, and w1 and w2 // are the angular velocity vectors of the two bodies. int s3 = 3; int s4 = 4; info.m_solverConstraints[s3].m_relpos1CrossNormal = p; info.m_solverConstraints[s4].m_relpos1CrossNormal = q; info.m_solverConstraints[s3].m_relpos2CrossNormal = -p; info.m_solverConstraints[s4].m_relpos2CrossNormal = -q; // compute the right hand side of the constraint equation. set relative // body velocities along p and q to bring the hinge back into alignment. // if ax1A,ax1B are the unit length hinge axes as computed from bodyA and // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2). // if "theta" is the angle between ax1 and ax2, we need an angular velocity // along u to cover angle erp*theta in one step : // |angular_velocity| = angle/time = erp*theta / stepsize // = (erp*fps) * theta // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) // ...as ax1 and ax2 are unit length. if theta is smallish, // theta ~= sin(theta), so // angular_velocity = (erp*fps) * (ax1 x ax2) // ax1 x ax2 is in the plane space of ax1, so we project the angular // velocity to p and q to find the right hand side. k = info.fps * info.erp; IndexedVector3 u = IndexedVector3.Cross(ref ax1A, ref ax1B); info.m_solverConstraints[s3].m_rhs = k * IndexedVector3.Dot(u, p); info.m_solverConstraints[s4].m_rhs = k * IndexedVector3.Dot(u, q); #endif // check angular limits nrow = 4; // last filled row int srow; float limit_err = 0f; int limit = 0; if (GetSolveLimit()) { #if _BT_USE_CENTER_LIMIT_ limit_err = m_limit.GetCorrection() * m_referenceSign; #else limit_err = m_correction * m_referenceSign; #endif limit = (limit_err > 0f) ? 1 : 2; } // if the hinge has joint limits or motor, add in the extra row bool powered = false; if (GetEnableAngularMotor()) { powered = true; } if (limit != 0 || powered) { nrow++; srow = nrow; info.m_solverConstraints[srow].m_relpos1CrossNormal = ax1; info.m_solverConstraints[srow].m_relpos2CrossNormal = -ax1; float lostop = GetLowerLimit(); float histop = GetUpperLimit(); if (limit != 0 && (MathUtil.CompareFloat(lostop, histop))) { // the joint motor is ineffective powered = false; } info.m_solverConstraints[srow].m_rhs = 0f; float currERP = ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_ERP_STOP) != 0) ? m_stopERP : info.erp; if (powered) { if ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_CFM_NORM) != 0) { info.m_solverConstraints[srow].m_cfm = m_normalCFM; } float mot_fact = GetMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info.fps * currERP); info.m_solverConstraints[srow].m_rhs += mot_fact * m_motorTargetVelocity * m_referenceSign; info.m_solverConstraints[srow].m_lowerLimit = -m_maxMotorImpulse; info.m_solverConstraints[srow].m_upperLimit = m_maxMotorImpulse; } if (limit != 0) { k = info.fps * currERP; info.m_solverConstraints[srow].m_rhs += k * limit_err; if ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_CFM_STOP) != 0) { info.m_solverConstraints[srow].m_cfm = m_stopCFM; } if (MathUtil.CompareFloat(lostop, histop)) { // limited low and high simultaneously info.m_solverConstraints[srow].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[srow].m_upperLimit = MathUtil.SIMD_INFINITY; } else if (limit == 1) { // low limit info.m_solverConstraints[srow].m_lowerLimit = 0; info.m_solverConstraints[srow].m_upperLimit = MathUtil.SIMD_INFINITY; } else { // high limit info.m_solverConstraints[srow].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[srow].m_upperLimit = 0; } // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) #if _BT_USE_CENTER_LIMIT_ float bounce = m_limit.GetRelaxationFactor(); #else float bounce = m_relaxationFactor; #endif if (bounce > 0f) { float vel = IndexedVector3.Dot(ref angVelA, ref ax1); vel -= IndexedVector3.Dot(ref angVelB, ref ax1); // only apply bounce if the velocity is incoming, and if the // resulting c[] exceeds what we already have. if (limit == 1) { // low limit if (vel < 0) { float newc = -bounce * vel; if (newc > info.m_solverConstraints[srow].m_rhs) { info.m_solverConstraints[srow].m_rhs = newc; } } } else { // high limit - all those computations are reversed if (vel > 0) { float newc = -bounce * vel; if (newc < info.m_solverConstraints[srow].m_rhs) { info.m_solverConstraints[srow].m_rhs = newc; } } } } #if _BT_USE_CENTER_LIMIT_ info.m_solverConstraints[srow].m_rhs *= m_limit.GetBiasFactor(); #else info.m_solverConstraints[srow].m_rhs *= m_biasFactor; #endif } // if(limit) } // if angular limit or powered #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints) { PrintInfo2(BulletGlobals.g_streamWriter, this, info); } #endif }
public void GetInfo2InternalUsingFrameOffset(ConstraintInfo2 info, ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedVector3 angVelA, ref IndexedVector3 angVelB) { // transforms in world space if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints) { MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "rbAFrame", m_rbAFrame); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "rbBFrame", m_rbBFrame); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transA", transA); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transB", transB); } IndexedMatrix trA = transA * m_rbAFrame; IndexedMatrix trB = transB * m_rbBFrame; if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints) { MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "trA", trA); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "trB", trB); } // pivot point IndexedVector3 pivotAInW = trA._origin; IndexedVector3 pivotBInW = trB._origin; #if true // difference between frames in WCS IndexedVector3 ofs = trB._origin - trA._origin; // now get weight factors depending on masses float miA = GetRigidBodyA().GetInvMass(); float miB = GetRigidBodyB().GetInvMass(); bool hasStaticBody = (miA < MathUtil.SIMD_EPSILON) || (miB < MathUtil.SIMD_EPSILON); float miS = miA + miB; float factA, factB; if (miS > 0.0f) { factA = miB / miS; } else { factA = 0.5f; } factB = 1.0f - factA; // get the desired direction of hinge axis // as weighted sum of Z-orthos of frameA and frameB in WCS IndexedVector3 ax1A = trA._basis.GetColumn(2); IndexedVector3 ax1B = trB._basis.GetColumn(2); IndexedVector3 ax1 = ax1A * factA + ax1B * factB; ax1.Normalize(); // fill first 3 rows // we want: velA + wA x relA == velB + wB x relB IndexedMatrix bodyA_trans = transA; IndexedMatrix bodyB_trans = transB; int s0 = 0; int s1 = 1; int s2 = 2; int nrow = 2; // last filled row IndexedVector3 tmpA, tmpB, relA, relB, p, q; // get vector from bodyB to frameB in WCS relB = trB._origin - bodyB_trans._origin; // get its projection to hinge axis IndexedVector3 projB = ax1 * IndexedVector3.Dot(relB, ax1); // get vector directed from bodyB to hinge axis (and orthogonal to it) IndexedVector3 orthoB = relB - projB; // same for bodyA relA = trA._origin - bodyA_trans._origin; IndexedVector3 projA = ax1 * IndexedVector3.Dot(relA, ax1); IndexedVector3 orthoA = relA - projA; IndexedVector3 totalDist = projA - projB; // get offset vectors relA and relB relA = orthoA + totalDist * factA; relB = orthoB - totalDist * factB; // now choose average ortho to hinge axis p = orthoB * factA + orthoA * factB; float len2 = p.LengthSquared(); if (len2 > MathUtil.SIMD_EPSILON) { p.Normalize(); } else { p = trA._basis.GetColumn(1); } // make one more ortho q = IndexedVector3.Cross(ax1, p); // fill three rows tmpA = IndexedVector3.Cross(relA, p); tmpB = IndexedVector3.Cross(relB, p); info.m_solverConstraints[s0].m_relpos1CrossNormal = tmpA; info.m_solverConstraints[s0].m_relpos2CrossNormal = -tmpB; tmpA = IndexedVector3.Cross(ref relA, ref q); tmpB = IndexedVector3.Cross(ref relB, ref q); if (hasStaticBody && GetSolveLimit()) { // to make constraint between static and dynamic objects more rigid // remove wA (or wB) from equation if angular limit is hit tmpB *= factB; tmpA *= factA; } info.m_solverConstraints[s1].m_relpos1CrossNormal = tmpA; info.m_solverConstraints[s1].m_relpos2CrossNormal = -tmpB; tmpA = IndexedVector3.Cross(ref relA, ref ax1); tmpB = IndexedVector3.Cross(ref relB, ref ax1); if (hasStaticBody) { // to make constraint between static and dynamic objects more rigid // remove wA (or wB) from equation tmpB *= factB; tmpA *= factA; } info.m_solverConstraints[s2].m_relpos1CrossNormal = tmpA; info.m_solverConstraints[s2].m_relpos2CrossNormal = -tmpB; float k = info.fps * info.erp; if (!m_angularOnly) { info.m_solverConstraints[s0].m_contactNormal = p; info.m_solverConstraints[s1].m_contactNormal = q; info.m_solverConstraints[s2].m_contactNormal = ax1; // compute three elements of right hand side float rhs = k * IndexedVector3.Dot(ref p, ref ofs); info.m_solverConstraints[s0].m_rhs = rhs; rhs = k * IndexedVector3.Dot(ref q, ref ofs); info.m_solverConstraints[s1].m_rhs = rhs; rhs = k * IndexedVector3.Dot(ref ax1, ref ofs); info.m_solverConstraints[s2].m_rhs = rhs; } // the hinge axis should be the only unconstrained // rotational axis, the angular velocity of the two bodies perpendicular to // the hinge axis should be equal. thus the constraint equations are // p*w1 - p*w2 = 0 // q*w1 - q*w2 = 0 // where p and q are unit vectors normal to the hinge axis, and w1 and w2 // are the angular velocity vectors of the two bodies. int s3 = 3; int s4 = 4; info.m_solverConstraints[s3].m_relpos1CrossNormal = p; info.m_solverConstraints[s4].m_relpos1CrossNormal = q; info.m_solverConstraints[s3].m_relpos2CrossNormal = -p; info.m_solverConstraints[s4].m_relpos2CrossNormal = -q; // compute the right hand side of the constraint equation. set relative // body velocities along p and q to bring the hinge back into alignment. // if ax1A,ax1B are the unit length hinge axes as computed from bodyA and // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2). // if "theta" is the angle between ax1 and ax2, we need an angular velocity // along u to cover angle erp*theta in one step : // |angular_velocity| = angle/time = erp*theta / stepsize // = (erp*fps) * theta // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) // ...as ax1 and ax2 are unit length. if theta is smallish, // theta ~= sin(theta), so // angular_velocity = (erp*fps) * (ax1 x ax2) // ax1 x ax2 is in the plane space of ax1, so we project the angular // velocity to p and q to find the right hand side. k = info.fps * info.erp; IndexedVector3 u = IndexedVector3.Cross(ref ax1A, ref ax1B); info.m_solverConstraints[s3].m_rhs = k * IndexedVector3.Dot(u, p); info.m_solverConstraints[s4].m_rhs = k * IndexedVector3.Dot(u, q); #endif // check angular limits nrow = 4; // last filled row int srow; float limit_err = 0f; int limit = 0; if (GetSolveLimit()) { #if _BT_USE_CENTER_LIMIT_ limit_err = m_limit.GetCorrection() * m_referenceSign; #else limit_err = m_correction * m_referenceSign; #endif limit = (limit_err > 0f) ? 1 : 2; } // if the hinge has joint limits or motor, add in the extra row bool powered = false; if (GetEnableAngularMotor()) { powered = true; } if (limit != 0 || powered) { nrow++; srow = nrow; info.m_solverConstraints[srow].m_relpos1CrossNormal = ax1; info.m_solverConstraints[srow].m_relpos2CrossNormal = -ax1; float lostop = GetLowerLimit(); float histop = GetUpperLimit(); if (limit != 0 && (MathUtil.CompareFloat(lostop, histop))) { // the joint motor is ineffective powered = false; } info.m_solverConstraints[srow].m_rhs = 0f; float currERP = ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_ERP_STOP) != 0) ? m_stopERP : info.erp; if (powered) { if ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_CFM_NORM) != 0) { info.m_solverConstraints[srow].m_cfm = m_normalCFM; } float mot_fact = GetMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info.fps * currERP); info.m_solverConstraints[srow].m_rhs += mot_fact * m_motorTargetVelocity * m_referenceSign; info.m_solverConstraints[srow].m_lowerLimit = -m_maxMotorImpulse; info.m_solverConstraints[srow].m_upperLimit = m_maxMotorImpulse; } if (limit != 0) { k = info.fps * currERP; info.m_solverConstraints[srow].m_rhs += k * limit_err; if ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_CFM_STOP) != 0) { info.m_solverConstraints[srow].m_cfm = m_stopCFM; } if (MathUtil.CompareFloat(lostop, histop)) { // limited low and high simultaneously info.m_solverConstraints[srow].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[srow].m_upperLimit = MathUtil.SIMD_INFINITY; } else if (limit == 1) { // low limit info.m_solverConstraints[srow].m_lowerLimit = 0; info.m_solverConstraints[srow].m_upperLimit = MathUtil.SIMD_INFINITY; } else { // high limit info.m_solverConstraints[srow].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[srow].m_upperLimit = 0; } // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) #if _BT_USE_CENTER_LIMIT_ float bounce = m_limit.GetRelaxationFactor(); #else float bounce = m_relaxationFactor; #endif if (bounce > 0f) { float vel = IndexedVector3.Dot(ref angVelA, ref ax1); vel -= IndexedVector3.Dot(ref angVelB, ref ax1); // only apply bounce if the velocity is incoming, and if the // resulting c[] exceeds what we already have. if (limit == 1) { // low limit if (vel < 0) { float newc = -bounce * vel; if (newc > info.m_solverConstraints[srow].m_rhs) { info.m_solverConstraints[srow].m_rhs = newc; } } } else { // high limit - all those computations are reversed if (vel > 0) { float newc = -bounce * vel; if (newc < info.m_solverConstraints[srow].m_rhs) { info.m_solverConstraints[srow].m_rhs = newc; } } } } #if _BT_USE_CENTER_LIMIT_ info.m_solverConstraints[srow].m_rhs *= m_limit.GetBiasFactor(); #else info.m_solverConstraints[srow].m_rhs *= m_biasFactor; #endif } // if(limit) } // if angular limit or powered if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints) { PrintInfo2(BulletGlobals.g_streamWriter, this, info); } }
public override void GetInfo2(ConstraintInfo2 info) { IndexedMatrix transA = m_rbA.GetCenterOfMassTransform(); IndexedMatrix transB = m_rbB.GetCenterOfMassTransform(); IndexedVector3 linVelA = m_rbA.GetLinearVelocity(); IndexedVector3 linVelB = m_rbB.GetLinearVelocity(); IndexedVector3 angVelA = m_rbA.GetAngularVelocity(); IndexedVector3 angVelB = m_rbB.GetAngularVelocity(); if (m_useOffsetForConstraintFrame) { // for stability better to solve angular limits first int row = SetAngularLimits(info, 0, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB); SetLinearLimits(info, row, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB); } else { // leave old version for compatibility int row = SetLinearLimits(info, 0, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB); SetAngularLimits(info, row, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB); } }
public void GetInfo2NonVirtual(ConstraintInfo2 info, IndexedMatrix transA, IndexedMatrix transB, IndexedVector3 linVelA, IndexedVector3 linVelB, float rbAinvMass, float rbBinvMass) { IndexedMatrix trA = GetCalculatedTransformA(); IndexedMatrix trB = GetCalculatedTransformB(); Debug.Assert(!m_useSolveConstraintObsolete); int i, s = 1; float signFact = m_useLinearReferenceFrameA ? 1.0f : -1.0f; // difference between frames in WCS IndexedVector3 ofs = trB._origin - trA._origin; // now get weight factors depending on masses float miA = rbAinvMass; float miB = rbBinvMass; bool hasStaticBody = (miA < MathUtil.SIMD_EPSILON) || (miB < MathUtil.SIMD_EPSILON); float miS = miA + miB; float factA, factB; if (miS > 0.0f) { factA = miB / miS; } else { factA = 0.5f; } factB = 1.0f - factA; IndexedVector3 ax1 = IndexedVector3.Zero, p, q; IndexedVector3 ax1A = trA._basis.GetColumn(0); IndexedVector3 ax1B = trB._basis.GetColumn(0); if (m_useOffsetForConstraintFrame) { // get the desired direction of slider axis // as weighted sum of X-orthos of frameA and frameB in WCS ax1 = ax1A * factA + ax1B * factB; ax1.Normalize(); // construct two orthos to slider axis TransformUtil.PlaneSpace1(ref ax1, out p, out q); } else { // old way - use frameA ax1 = trA._basis.GetColumn(0); // get 2 orthos to slider axis (Y, Z) p = trA._basis.GetColumn(1); q = trA._basis.GetColumn(2); } // make rotations around these orthos equal // the slider axis should be the only unconstrained // rotational axis, the angular velocity of the two bodies perpendicular to // the slider axis should be equal. thus the constraint equations are // p*w1 - p*w2 = 0 // q*w1 - q*w2 = 0 // where p and q are unit vectors normal to the slider axis, and w1 and w2 // are the angular velocity vectors of the two bodies. info.m_solverConstraints[0].m_relpos1CrossNormal = p; info.m_solverConstraints[s].m_relpos1CrossNormal = q; info.m_solverConstraints[0].m_relpos2CrossNormal = -p; info.m_solverConstraints[s].m_relpos2CrossNormal = -q; // compute the right hand side of the constraint equation. set relative // body velocities along p and q to bring the slider back into alignment. // if ax1A,ax1B are the unit length slider axes as computed from bodyA and // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2). // if "theta" is the angle between ax1 and ax2, we need an angular velocity // along u to cover angle erp*theta in one step : // |angular_velocity| = angle/time = erp*theta / stepsize // = (erp*fps) * theta // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) // ...as ax1 and ax2 are unit length. if theta is smallish, // theta ~= sin(theta), so // angular_velocity = (erp*fps) * (ax1 x ax2) // ax1 x ax2 is in the plane space of ax1, so we project the angular // velocity to p and q to find the right hand side. // float k = info.fps * info.erp * getSoftnessOrthoAng(); float currERP = ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_ERP_ORTANG) != 0) ? m_softnessOrthoAng : m_softnessOrthoAng * info.erp; float k = info.fps * currERP; IndexedVector3 u = IndexedVector3.Cross(ax1A, ax1B); info.m_solverConstraints[0].m_rhs = k * IndexedVector3.Dot(u, p); info.m_solverConstraints[s].m_rhs = k * IndexedVector3.Dot(u, q); if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_ORTANG) != 0) { info.m_solverConstraints[0].m_cfm = m_cfmOrthoAng; info.m_solverConstraints[s].m_cfm = m_cfmOrthoAng; } int nrow = 1; // last filled row int srow = nrow; float limit_err; int limit; bool powered; // next two rows. // we want: velA + wA x relA == velB + wB x relB ... but this would // result in three equations, so we project along two orthos to the slider axis IndexedMatrix bodyA_trans = transA; IndexedMatrix bodyB_trans = transB; nrow++; int s2 = nrow * s; nrow++; int s3 = nrow * s; IndexedVector3 tmpA = IndexedVector3.Zero, tmpB = IndexedVector3.Zero, relA = IndexedVector3.Zero, relB = IndexedVector3.Zero, c = IndexedVector3.Zero; if (m_useOffsetForConstraintFrame) { // get vector from bodyB to frameB in WCS relB = trB._origin - bodyB_trans._origin; // get its projection to slider axis IndexedVector3 projB = ax1 * IndexedVector3.Dot(relB, ax1); // get vector directed from bodyB to slider axis (and orthogonal to it) IndexedVector3 orthoB = relB - projB; // same for bodyA relA = trA._origin - bodyA_trans._origin; IndexedVector3 projA = ax1 * IndexedVector3.Dot(relA, ax1); IndexedVector3 orthoA = relA - projA; // get desired offset between frames A and B along slider axis float sliderOffs = m_linPos - m_depth.X; // desired vector from projection of center of bodyA to projection of center of bodyB to slider axis IndexedVector3 totalDist = projA + ax1 * sliderOffs - projB; // get offset vectors relA and relB relA = orthoA + totalDist * factA; relB = orthoB - totalDist * factB; // now choose average ortho to slider axis p = orthoB * factA + orthoA * factB; float len2 = p.LengthSquared(); if (len2 > MathUtil.SIMD_EPSILON) { p.Normalize(); } else { p = trA._basis.GetColumn(1); } // make one more ortho q = IndexedVector3.Cross(ax1, p); // fill two rows tmpA = IndexedVector3.Cross(relA, p); tmpB = IndexedVector3.Cross(relB, p); info.m_solverConstraints[s2].m_relpos1CrossNormal = tmpA; info.m_solverConstraints[s2].m_relpos2CrossNormal = -tmpB; tmpA = IndexedVector3.Cross(relA, q); tmpB = IndexedVector3.Cross(relB, q); if (hasStaticBody && GetSolveAngLimit()) { // to make constraint between static and dynamic objects more rigid // remove wA (or wB) from equation if angular limit is hit tmpB *= factB; tmpA *= factA; } info.m_solverConstraints[s3].m_relpos1CrossNormal = tmpA; info.m_solverConstraints[s3].m_relpos2CrossNormal = -tmpB; info.m_solverConstraints[s2].m_contactNormal = p; info.m_solverConstraints[s3].m_contactNormal = q; } else { // old way - maybe incorrect if bodies are not on the slider axis // see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0 IndexedVector3 tmp = IndexedVector3.Cross(c, p); info.m_solverConstraints[s2].m_relpos1CrossNormal = factA * tmp; info.m_solverConstraints[s2].m_relpos2CrossNormal = factB * tmp; tmp = IndexedVector3.Cross(c, q); info.m_solverConstraints[s3].m_relpos1CrossNormal = factA * tmp; info.m_solverConstraints[s3].m_relpos2CrossNormal = factB * tmp; info.m_solverConstraints[s2].m_contactNormal = p; info.m_solverConstraints[s3].m_contactNormal = q; } // compute two elements of right hand side // k = info.fps * info.erp * getSoftnessOrthoLin(); currERP = ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_ERP_ORTLIN) != 0) ? m_softnessOrthoLin : m_softnessOrthoLin * info.erp; k = info.fps * currERP; float rhs = k * IndexedVector3.Dot(p, ofs); info.m_solverConstraints[s2].m_rhs = rhs; rhs = k * IndexedVector3.Dot(q, ofs); info.m_solverConstraints[s3].m_rhs = rhs; if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_ORTLIN) != 0) { info.m_solverConstraints[s2].m_cfm = m_cfmOrthoLin; info.m_solverConstraints[s3].m_cfm = m_cfmOrthoLin; } // check linear limits limit_err = 0.0f; limit = 0; if (GetSolveLinLimit()) { limit_err = GetLinDepth() * signFact; limit = (limit_err > 0f) ? 2 : 1; } powered = false; if (GetPoweredLinMotor()) { powered = true; } // if the slider has joint limits or motor, add in the extra row if (limit != 0 || powered) { nrow++; srow = nrow; info.m_solverConstraints[srow].m_contactNormal = ax1; // linear torque decoupling step: // // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies // do not create a torque couple. in other words, the points that the // constraint force is applied at must lie along the same ax1 axis. // a torque couple will result in limited slider-jointed free // bodies from gaining angular momentum. if (m_useOffsetForConstraintFrame) { // this is needed only when bodyA and bodyB are both dynamic. if (!hasStaticBody) { tmpA = IndexedVector3.Cross(relA, ax1); tmpB = IndexedVector3.Cross(relB, ax1); info.m_solverConstraints[srow].m_relpos1CrossNormal = tmpA; info.m_solverConstraints[srow].m_relpos2CrossNormal = -tmpB; } } else { // The old way. May be incorrect if bodies are not on the slider axis IndexedVector3 ltd = IndexedVector3.Cross(c, ax1); // Linear Torque Decoupling vector (a torque) info.m_solverConstraints[nrow].m_relpos1CrossNormal = factA * ltd; info.m_solverConstraints[nrow].m_relpos2CrossNormal = factB * ltd; } // right-hand part float lostop = GetLowerLinLimit(); float histop = GetUpperLinLimit(); if (limit != 0 && (MathUtil.CompareFloat(lostop, histop))) { // the joint motor is ineffective powered = false; } info.m_solverConstraints[nrow].m_rhs = 0f; info.m_solverConstraints[nrow].m_lowerLimit = 0f; info.m_solverConstraints[nrow].m_upperLimit = 0f; currERP = ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_ERP_LIMLIN) != 0) ? m_softnessLimLin : info.erp; if (powered) { if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_DIRLIN) != 0) { info.m_solverConstraints[nrow].m_cfm = m_cfmDirLin; } float tag_vel = GetTargetLinMotorVelocity(); float mot_fact = GetMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info.fps * currERP); info.m_solverConstraints[nrow].m_rhs -= signFact * mot_fact * GetTargetLinMotorVelocity(); info.m_solverConstraints[nrow].m_lowerLimit += -GetMaxLinMotorForce() * info.fps; info.m_solverConstraints[nrow].m_upperLimit += GetMaxLinMotorForce() * info.fps; } if (limit != 0) { k = info.fps * currERP; info.m_solverConstraints[nrow].m_rhs += k * limit_err; if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_LIMLIN) != 0) { info.m_solverConstraints[nrow].m_cfm = m_cfmLimLin; } if (MathUtil.CompareFloat(lostop, histop)) { // limited low and high simultaneously info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY; } else if (limit == 1) { // low limit info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[nrow].m_upperLimit = 0f; } else { // high limit info.m_solverConstraints[nrow].m_lowerLimit = 0f; info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY; } // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that) float bounce = Math.Abs(1.0f - GetDampingLimLin()); if (bounce > 0.0f) { float vel = IndexedVector3.Dot(linVelA, ax1); vel -= IndexedVector3.Dot(linVelB, ax1); vel *= signFact; // only apply bounce if the velocity is incoming, and if the // resulting c[] exceeds what we already have. if (limit == 1) { // low limit if (vel < 0) { float newc = -bounce * vel; if (newc > info.m_solverConstraints[nrow].m_rhs) { info.m_solverConstraints[nrow].m_rhs = newc; } } } else { // high limit - all those computations are reversed if (vel > 0) { float newc = -bounce * vel; if (newc < info.m_solverConstraints[nrow].m_rhs) { info.m_solverConstraints[nrow].m_rhs = newc; } } } } info.m_solverConstraints[nrow].m_rhs *= GetSoftnessLimLin(); } // if(limit) } // if linear limit // check angular limits limit_err = 0.0f; limit = 0; if (GetSolveAngLimit()) { limit_err = GetAngDepth(); limit = (limit_err > 0.0f) ? 1 : 2; } // if the slider has joint limits, add in the extra row powered = false; if (GetPoweredAngMotor()) { powered = true; } if (limit != 0 || powered) { nrow++; srow = nrow; info.m_solverConstraints[srow].m_relpos1CrossNormal = ax1; info.m_solverConstraints[srow].m_relpos2CrossNormal = -ax1; float lostop = GetLowerAngLimit(); float histop = GetUpperAngLimit(); if (limit != 0 && (MathUtil.CompareFloat(lostop, histop))) { // the joint motor is ineffective powered = false; } currERP = ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_ERP_LIMANG) != 0) ? m_softnessLimAng : info.erp; if (powered) { if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_DIRANG) != 0) { info.m_solverConstraints[nrow].m_cfm = m_cfmDirAng; } float mot_fact = GetMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, GetTargetAngMotorVelocity(), info.fps * currERP); info.m_solverConstraints[nrow].m_rhs = mot_fact * GetTargetAngMotorVelocity(); info.m_solverConstraints[nrow].m_lowerLimit = -GetMaxAngMotorForce() * info.fps; info.m_solverConstraints[nrow].m_upperLimit = GetMaxAngMotorForce() * info.fps; } if (limit != 0) { k = info.fps * currERP; info.m_solverConstraints[nrow].m_rhs += k * limit_err; if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_LIMANG) != 0) { info.m_solverConstraints[nrow].m_cfm = m_cfmLimAng; } if (MathUtil.CompareFloat(lostop, histop)) { // limited low and high simultaneously info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY; } else if (limit == 1) { // low limit info.m_solverConstraints[nrow].m_lowerLimit = 0; info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY; } else { // high limit info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[nrow].m_upperLimit = 0; } // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) float bounce = Math.Abs(1.0f - GetDampingLimAng()); if (bounce > 0.0f) { float vel = IndexedVector3.Dot(m_rbA.GetAngularVelocity(), ax1); vel -= IndexedVector3.Dot(m_rbB.GetAngularVelocity(), ax1); // only apply bounce if the velocity is incoming, and if the // resulting c[] exceeds what we already have. if (limit == 1) { // low limit if (vel < 0) { float newc = -bounce * vel; if (newc > info.m_solverConstraints[nrow].m_rhs) { info.m_solverConstraints[nrow].m_rhs = newc; } } } else { // high limit - all those computations are reversed if (vel > 0) { float newc = -bounce * vel; if (newc < info.m_solverConstraints[nrow].m_rhs) { info.m_solverConstraints[nrow].m_rhs = newc; } } } } info.m_solverConstraints[nrow].m_rhs *= GetSoftnessLimAng(); } // if(limit) } // if angular limit or powered #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints) { PrintInfo2(BulletGlobals.g_streamWriter, this, info); } #endif }
public void GetInfo2NonVirtual(ConstraintInfo2 info, IndexedMatrix transA, IndexedMatrix transB, IndexedVector3 linVelA, IndexedVector3 linVelB, IndexedVector3 angVelA, IndexedVector3 angVelB) { //prepare constraint CalculateTransforms(ref transA, ref transB); for (int i = 0; i < 3; i++) { TestAngularLimitMotor(i); } if (m_useOffsetForConstraintFrame) { // for stability better to solve angular limits first int row = SetAngularLimits(info, 0, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB); SetLinearLimits(info, row, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB); } else { // leave old version for compatibility int row = SetLinearLimits(info, 0, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB); SetAngularLimits(info, row, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB); } }
public void TestLinLimits2(ConstraintInfo2 info) { }
public virtual int GetLimitMotorInfo2(RotationalLimitMotor limot, ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedVector3 linVelA, ref IndexedVector3 linVelB, ref IndexedVector3 angVelA, ref IndexedVector3 angVelB, ConstraintInfo2 info, int row, ref IndexedVector3 ax1, int rotational, bool rotAllowed) { bool powered = limot.m_enableMotor; int limit = limot.m_currentLimit; if (powered || limit != 0) { // if the joint is powered, or has joint limits, add in the extra row //float* J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis; //float* J2 = rotational ? info->m_J2angularAxis : 0; //info2.m_J1linearAxis = currentConstraintRow->m_contactNormal; //info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal; //info2.m_J2linearAxis = 0; //info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal; if (rotational != 0) { info.m_solverConstraints[row].m_relpos1CrossNormal = ax1; MathUtil.ZeroCheckVector(info.m_solverConstraints[row].m_relpos1CrossNormal); } else { info.m_solverConstraints[row].m_contactNormal = ax1; MathUtil.ZeroCheckVector(info.m_solverConstraints[row].m_contactNormal); } if (rotational != 0) { info.m_solverConstraints[row].m_relpos2CrossNormal = -ax1; } //MathUtil.zeroCheckVector(info.m_solverConstraints[row].m_relpos2CrossNormal); if (rotational == 0) { if (m_useOffsetForConstraintFrame) { IndexedVector3 tmpA = IndexedVector3.Zero, tmpB = IndexedVector3.Zero, relA = IndexedVector3.Zero, relB = IndexedVector3.Zero; // get vector from bodyB to frameB in WCS relB = m_calculatedTransformB._origin - transB._origin; // get its projection to constraint axis IndexedVector3 projB = ax1 * IndexedVector3.Dot(relB, ax1); // get vector directed from bodyB to constraint axis (and orthogonal to it) IndexedVector3 orthoB = relB - projB; // same for bodyA relA = m_calculatedTransformA._origin - transA._origin; IndexedVector3 projA = ax1 * IndexedVector3.Dot(relA, ax1); IndexedVector3 orthoA = relA - projA; // get desired offset between frames A and B along constraint axis float desiredOffs = limot.m_currentPosition - limot.m_currentLimitError; // desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis IndexedVector3 totalDist = projA + ax1 * desiredOffs - projB; // get offset vectors relA and relB relA = orthoA + totalDist * m_factA; relB = orthoB - totalDist * m_factB; tmpA = IndexedVector3.Cross(relA, ax1); tmpB = IndexedVector3.Cross(relB, ax1); if (m_hasStaticBody && (!rotAllowed)) { tmpA *= m_factA; tmpB *= m_factB; } info.m_solverConstraints[row].m_relpos1CrossNormal = tmpA; MathUtil.ZeroCheckVector(ref tmpA); info.m_solverConstraints[row].m_relpos2CrossNormal = -tmpB; MathUtil.ZeroCheckVector(ref tmpB); } else { IndexedVector3 ltd; // Linear Torque Decoupling vector IndexedVector3 c = m_calculatedTransformB._origin - transA._origin; ltd = IndexedVector3.Cross(c, ax1); info.m_solverConstraints[row].m_relpos1CrossNormal = ltd; MathUtil.ZeroCheckVector(info.m_solverConstraints[row].m_relpos1CrossNormal); c = m_calculatedTransformB._origin - transB._origin; ltd = -IndexedVector3.Cross(c, ax1); info.m_solverConstraints[row].m_relpos2CrossNormal = ltd; MathUtil.ZeroCheckVector(info.m_solverConstraints[row].m_relpos2CrossNormal); } } // if we're limited low and high simultaneously, the joint motor is // ineffective if (limit != 0 && (MathUtil.CompareFloat(limot.m_loLimit, limot.m_hiLimit))) { powered = false; } info.m_solverConstraints[row].m_rhs = 0f; if (powered) { info.m_solverConstraints[row].m_cfm = limot.m_normalCFM; if (limit == 0) { float tag_vel = (rotational != 0) ? limot.m_targetVelocity : -limot.m_targetVelocity; float mot_fact = GetMotorFactor(limot.m_currentPosition, limot.m_loLimit, limot.m_hiLimit, tag_vel, info.fps * limot.m_stopERP); info.m_solverConstraints[row].m_rhs += mot_fact * limot.m_targetVelocity; info.m_solverConstraints[row].m_lowerLimit = -limot.m_maxMotorForce; info.m_solverConstraints[row].m_upperLimit = limot.m_maxMotorForce; } } if (limit != 0) { float k = info.fps * limot.m_stopERP; if (rotational == 0) { info.m_solverConstraints[row].m_rhs += k * limot.m_currentLimitError; } else { info.m_solverConstraints[row].m_rhs += -k * limot.m_currentLimitError; } info.m_solverConstraints[row].m_cfm = limot.m_stopCFM; if (MathUtil.CompareFloat(limot.m_loLimit, limot.m_hiLimit)) { // limited low and high simultaneously info.m_solverConstraints[row].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[row].m_upperLimit = MathUtil.SIMD_INFINITY; } else { if (limit == 1) { info.m_solverConstraints[row].m_lowerLimit = 0; info.m_solverConstraints[row].m_upperLimit = MathUtil.SIMD_INFINITY; } else { info.m_solverConstraints[row].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[row].m_upperLimit = 0; } // deal with bounce if (limot.m_bounce > 0) { // calculate joint velocity float vel; if (rotational != 0) { vel = IndexedVector3.Dot(angVelA, ax1); vel -= IndexedVector3.Dot(angVelB, ax1); } else { vel = IndexedVector3.Dot(linVelA, ax1); vel -= IndexedVector3.Dot(linVelB, ax1); } // only apply bounce if the velocity is incoming, and if the // resulting c[] exceeds what we already have. if (limit == 1) { if (vel < 0) { float newc = -limot.m_bounce * vel; if (newc > info.m_solverConstraints[row].m_rhs) { info.m_solverConstraints[row].m_rhs = newc; } } } else { if (vel > 0) { float newc = -limot.m_bounce * vel; if (newc < info.m_solverConstraints[row].m_rhs) { info.m_solverConstraints[row].m_rhs = newc; } } } } } } return 1; } else return 0; }
public void GetInfo2NonVirtual(ConstraintInfo2 info, IndexedMatrix transA, IndexedMatrix transB, IndexedVector3 linVelA, IndexedVector3 linVelB, float rbAinvMass, float rbBinvMass) { IndexedMatrix trA = GetCalculatedTransformA(); IndexedMatrix trB = GetCalculatedTransformB(); Debug.Assert(!m_useSolveConstraintObsolete); int i, s = 1; float signFact = m_useLinearReferenceFrameA ? 1.0f : -1.0f; // difference between frames in WCS IndexedVector3 ofs = trB._origin - trA._origin; // now get weight factors depending on masses float miA = rbAinvMass; float miB = rbBinvMass; bool hasStaticBody = (miA < MathUtil.SIMD_EPSILON) || (miB < MathUtil.SIMD_EPSILON); float miS = miA + miB; float factA, factB; if (miS > 0.0f) { factA = miB / miS; } else { factA = 0.5f; } factB = 1.0f - factA; IndexedVector3 ax1 = IndexedVector3.Zero, p, q; IndexedVector3 ax1A = trA._basis.GetColumn(0); IndexedVector3 ax1B = trB._basis.GetColumn(0); if (m_useOffsetForConstraintFrame) { // get the desired direction of slider axis // as weighted sum of X-orthos of frameA and frameB in WCS ax1 = ax1A * factA + ax1B * factB; ax1.Normalize(); // construct two orthos to slider axis TransformUtil.PlaneSpace1(ref ax1, out p, out q); } else { // old way - use frameA ax1 = trA._basis.GetColumn(0); // get 2 orthos to slider axis (Y, Z) p = trA._basis.GetColumn(1); q = trA._basis.GetColumn(2); } // make rotations around these orthos equal // the slider axis should be the only unconstrained // rotational axis, the angular velocity of the two bodies perpendicular to // the slider axis should be equal. thus the constraint equations are // p*w1 - p*w2 = 0 // q*w1 - q*w2 = 0 // where p and q are unit vectors normal to the slider axis, and w1 and w2 // are the angular velocity vectors of the two bodies. info.m_solverConstraints[0].m_relpos1CrossNormal = p; info.m_solverConstraints[s].m_relpos1CrossNormal = q; info.m_solverConstraints[0].m_relpos2CrossNormal = -p; info.m_solverConstraints[s].m_relpos2CrossNormal = -q; // compute the right hand side of the constraint equation. set relative // body velocities along p and q to bring the slider back into alignment. // if ax1A,ax1B are the unit length slider axes as computed from bodyA and // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2). // if "theta" is the angle between ax1 and ax2, we need an angular velocity // along u to cover angle erp*theta in one step : // |angular_velocity| = angle/time = erp*theta / stepsize // = (erp*fps) * theta // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) // ...as ax1 and ax2 are unit length. if theta is smallish, // theta ~= sin(theta), so // angular_velocity = (erp*fps) * (ax1 x ax2) // ax1 x ax2 is in the plane space of ax1, so we project the angular // velocity to p and q to find the right hand side. // float k = info.fps * info.erp * getSoftnessOrthoAng(); float currERP = ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_ERP_ORTANG) != 0) ? m_softnessOrthoAng : m_softnessOrthoAng * info.erp; float k = info.fps * currERP; IndexedVector3 u = IndexedVector3.Cross(ax1A, ax1B); info.m_solverConstraints[0].m_rhs = k * IndexedVector3.Dot(u, p); info.m_solverConstraints[s].m_rhs = k * IndexedVector3.Dot(u, q); if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_ORTANG) != 0) { info.m_solverConstraints[0].m_cfm = m_cfmOrthoAng; info.m_solverConstraints[s].m_cfm = m_cfmOrthoAng; } int nrow = 1; // last filled row int srow = nrow; float limit_err; int limit; bool powered; // next two rows. // we want: velA + wA x relA == velB + wB x relB ... but this would // result in three equations, so we project along two orthos to the slider axis IndexedMatrix bodyA_trans = transA; IndexedMatrix bodyB_trans = transB; nrow++; int s2 = nrow * s; nrow++; int s3 = nrow * s; IndexedVector3 tmpA = IndexedVector3.Zero, tmpB = IndexedVector3.Zero, relA = IndexedVector3.Zero, relB = IndexedVector3.Zero, c = IndexedVector3.Zero; if (m_useOffsetForConstraintFrame) { // get vector from bodyB to frameB in WCS relB = trB._origin - bodyB_trans._origin; // get its projection to slider axis IndexedVector3 projB = ax1 * IndexedVector3.Dot(relB, ax1); // get vector directed from bodyB to slider axis (and orthogonal to it) IndexedVector3 orthoB = relB - projB; // same for bodyA relA = trA._origin - bodyA_trans._origin; IndexedVector3 projA = ax1 * IndexedVector3.Dot(relA, ax1); IndexedVector3 orthoA = relA - projA; // get desired offset between frames A and B along slider axis float sliderOffs = m_linPos - m_depth.X; // desired vector from projection of center of bodyA to projection of center of bodyB to slider axis IndexedVector3 totalDist = projA + ax1 * sliderOffs - projB; // get offset vectors relA and relB relA = orthoA + totalDist * factA; relB = orthoB - totalDist * factB; // now choose average ortho to slider axis p = orthoB * factA + orthoA * factB; float len2 = p.LengthSquared(); if (len2 > MathUtil.SIMD_EPSILON) { p.Normalize(); } else { p = trA._basis.GetColumn(1); } // make one more ortho q = IndexedVector3.Cross(ax1, p); // fill two rows tmpA = IndexedVector3.Cross(relA, p); tmpB = IndexedVector3.Cross(relB, p); info.m_solverConstraints[s2].m_relpos1CrossNormal = tmpA; info.m_solverConstraints[s2].m_relpos2CrossNormal = -tmpB; tmpA = IndexedVector3.Cross(relA, q); tmpB = IndexedVector3.Cross(relB, q); if (hasStaticBody && GetSolveAngLimit()) { // to make constraint between static and dynamic objects more rigid // remove wA (or wB) from equation if angular limit is hit tmpB *= factB; tmpA *= factA; } info.m_solverConstraints[s3].m_relpos1CrossNormal = tmpA; info.m_solverConstraints[s3].m_relpos2CrossNormal = -tmpB; info.m_solverConstraints[s2].m_contactNormal = p; info.m_solverConstraints[s3].m_contactNormal = q; } else { // old way - maybe incorrect if bodies are not on the slider axis // see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0 IndexedVector3 tmp = IndexedVector3.Cross(c, p); info.m_solverConstraints[s2].m_relpos1CrossNormal = factA * tmp; info.m_solverConstraints[s2].m_relpos2CrossNormal = factB * tmp; tmp = IndexedVector3.Cross(c, q); info.m_solverConstraints[s3].m_relpos1CrossNormal = factA * tmp; info.m_solverConstraints[s3].m_relpos2CrossNormal = factB * tmp; info.m_solverConstraints[s2].m_contactNormal = p; info.m_solverConstraints[s3].m_contactNormal = q; } // compute two elements of right hand side // k = info.fps * info.erp * getSoftnessOrthoLin(); currERP = ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_ERP_ORTLIN) != 0) ? m_softnessOrthoLin : m_softnessOrthoLin * info.erp; k = info.fps * currERP; float rhs = k * IndexedVector3.Dot(p, ofs); info.m_solverConstraints[s2].m_rhs = rhs; rhs = k * IndexedVector3.Dot(q, ofs); info.m_solverConstraints[s3].m_rhs = rhs; if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_ORTLIN) != 0) { info.m_solverConstraints[s2].m_cfm = m_cfmOrthoLin; info.m_solverConstraints[s3].m_cfm = m_cfmOrthoLin; } // check linear limits limit_err = 0.0f; limit = 0; if (GetSolveLinLimit()) { limit_err = GetLinDepth() * signFact; limit = (limit_err > 0f) ? 2 : 1; } powered = false; if (GetPoweredLinMotor()) { powered = true; } // if the slider has joint limits or motor, add in the extra row if (limit != 0 || powered) { nrow++; srow = nrow; info.m_solverConstraints[srow].m_contactNormal = ax1; // linear torque decoupling step: // // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies // do not create a torque couple. in other words, the points that the // constraint force is applied at must lie along the same ax1 axis. // a torque couple will result in limited slider-jointed free // bodies from gaining angular momentum. if (m_useOffsetForConstraintFrame) { // this is needed only when bodyA and bodyB are both dynamic. if (!hasStaticBody) { tmpA = IndexedVector3.Cross(relA, ax1); tmpB = IndexedVector3.Cross(relB, ax1); info.m_solverConstraints[srow].m_relpos1CrossNormal = tmpA; info.m_solverConstraints[srow].m_relpos2CrossNormal = -tmpB; } } else { // The old way. May be incorrect if bodies are not on the slider axis IndexedVector3 ltd = IndexedVector3.Cross(c, ax1); // Linear Torque Decoupling vector (a torque) info.m_solverConstraints[nrow].m_relpos1CrossNormal = factA * ltd; info.m_solverConstraints[nrow].m_relpos2CrossNormal = factB * ltd; } // right-hand part float lostop = GetLowerLinLimit(); float histop = GetUpperLinLimit(); if (limit != 0 && (MathUtil.CompareFloat(lostop, histop))) { // the joint motor is ineffective powered = false; } info.m_solverConstraints[nrow].m_rhs = 0f; info.m_solverConstraints[nrow].m_lowerLimit = 0f; info.m_solverConstraints[nrow].m_upperLimit = 0f; currERP = ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_ERP_LIMLIN) != 0) ? m_softnessLimLin : info.erp; if (powered) { if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_DIRLIN) != 0) { info.m_solverConstraints[nrow].m_cfm = m_cfmDirLin; } float tag_vel = GetTargetLinMotorVelocity(); float mot_fact = GetMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info.fps * currERP); info.m_solverConstraints[nrow].m_rhs -= signFact * mot_fact * GetTargetLinMotorVelocity(); info.m_solverConstraints[nrow].m_lowerLimit += -GetMaxLinMotorForce() * info.fps; info.m_solverConstraints[nrow].m_upperLimit += GetMaxLinMotorForce() * info.fps; } if (limit != 0) { k = info.fps * currERP; info.m_solverConstraints[nrow].m_rhs += k * limit_err; if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_LIMLIN) != 0) { info.m_solverConstraints[nrow].m_cfm = m_cfmLimLin; } if (MathUtil.CompareFloat(lostop, histop)) { // limited low and high simultaneously info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY; } else if (limit == 1) { // low limit info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[nrow].m_upperLimit = 0f; } else { // high limit info.m_solverConstraints[nrow].m_lowerLimit = 0f; info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY; } // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that) float bounce = Math.Abs(1.0f - GetDampingLimLin()); if (bounce > 0.0f) { float vel = IndexedVector3.Dot(linVelA, ax1); vel -= IndexedVector3.Dot(linVelB, ax1); vel *= signFact; // only apply bounce if the velocity is incoming, and if the // resulting c[] exceeds what we already have. if (limit == 1) { // low limit if (vel < 0) { float newc = -bounce * vel; if (newc > info.m_solverConstraints[nrow].m_rhs) { info.m_solverConstraints[nrow].m_rhs = newc; } } } else { // high limit - all those computations are reversed if (vel > 0) { float newc = -bounce * vel; if (newc < info.m_solverConstraints[nrow].m_rhs) { info.m_solverConstraints[nrow].m_rhs = newc; } } } } info.m_solverConstraints[nrow].m_rhs *= GetSoftnessLimLin(); } // if(limit) } // if linear limit // check angular limits limit_err = 0.0f; limit = 0; if (GetSolveAngLimit()) { limit_err = GetAngDepth(); limit = (limit_err > 0.0f) ? 1 : 2; } // if the slider has joint limits, add in the extra row powered = false; if (GetPoweredAngMotor()) { powered = true; } if (limit != 0 || powered) { nrow++; srow = nrow; info.m_solverConstraints[srow].m_relpos1CrossNormal = ax1; info.m_solverConstraints[srow].m_relpos2CrossNormal = -ax1; float lostop = GetLowerAngLimit(); float histop = GetUpperAngLimit(); if (limit != 0 && (MathUtil.CompareFloat(lostop, histop))) { // the joint motor is ineffective powered = false; } currERP = ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_ERP_LIMANG) != 0) ? m_softnessLimAng : info.erp; if (powered) { if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_DIRANG) != 0) { info.m_solverConstraints[nrow].m_cfm = m_cfmDirAng; } float mot_fact = GetMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, GetTargetAngMotorVelocity(), info.fps * currERP); info.m_solverConstraints[nrow].m_rhs = mot_fact * GetTargetAngMotorVelocity(); info.m_solverConstraints[nrow].m_lowerLimit = -GetMaxAngMotorForce() * info.fps; info.m_solverConstraints[nrow].m_upperLimit = GetMaxAngMotorForce() * info.fps; } if (limit != 0) { k = info.fps * currERP; info.m_solverConstraints[nrow].m_rhs += k * limit_err; if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_LIMANG) != 0) { info.m_solverConstraints[nrow].m_cfm = m_cfmLimAng; } if (MathUtil.CompareFloat(lostop, histop)) { // limited low and high simultaneously info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY; } else if (limit == 1) { // low limit info.m_solverConstraints[nrow].m_lowerLimit = 0; info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY; } else { // high limit info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY; info.m_solverConstraints[nrow].m_upperLimit = 0; } // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) float bounce = Math.Abs(1.0f - GetDampingLimAng()); if (bounce > 0.0f) { float vel = IndexedVector3.Dot(m_rbA.GetAngularVelocity(), ax1); vel -= IndexedVector3.Dot(m_rbB.GetAngularVelocity(), ax1); // only apply bounce if the velocity is incoming, and if the // resulting c[] exceeds what we already have. if (limit == 1) { // low limit if (vel < 0) { float newc = -bounce * vel; if (newc > info.m_solverConstraints[nrow].m_rhs) { info.m_solverConstraints[nrow].m_rhs = newc; } } } else { // high limit - all those computations are reversed if (vel > 0) { float newc = -bounce * vel; if (newc < info.m_solverConstraints[nrow].m_rhs) { info.m_solverConstraints[nrow].m_rhs = newc; } } } } info.m_solverConstraints[nrow].m_rhs *= GetSoftnessLimAng(); } // if(limit) } // if angular limit or powered if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints) { PrintInfo2(BulletGlobals.g_streamWriter, this, info); } }
protected virtual int SetLinearLimits(ConstraintInfo2 info, int row, ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedVector3 linVelA, ref IndexedVector3 linVelB, ref IndexedVector3 angVelA, ref IndexedVector3 angVelB) { //solve linear limits RotationalLimitMotor limot = new RotationalLimitMotor(); for (int i = 0; i < 3; i++) { if (m_linearLimits.NeedApplyForce(i)) { // re-use rotational motor code limot.m_bounce = 0f; limot.m_currentLimit = m_linearLimits.m_currentLimit[i]; limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i]; limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i]; limot.m_damping = m_linearLimits.m_damping; limot.m_enableMotor = m_linearLimits.m_enableMotor[i]; limot.m_hiLimit = m_linearLimits.m_upperLimit[i]; limot.m_limitSoftness = m_linearLimits.m_limitSoftness; limot.m_loLimit = m_linearLimits.m_lowerLimit[i]; limot.m_maxLimitForce = 0.0f; limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i]; limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i]; IndexedVector3 axis = m_calculatedTransformA._basis.GetColumn(i); int tempFlags = (((int)m_flags) >> (i * BT_6DOF_FLAGS_AXIS_SHIFT)); SixDofFlags flags = (SixDofFlags)tempFlags; limot.m_normalCFM = ((flags & SixDofFlags.BT_6DOF_FLAGS_CFM_NORM) != 0) ? m_linearLimits.m_normalCFM[i] : info.m_solverConstraints[0].m_cfm; limot.m_stopCFM = ((flags & SixDofFlags.BT_6DOF_FLAGS_CFM_STOP) != 0) ? m_linearLimits.m_stopCFM[i] : info.m_solverConstraints[0].m_cfm; limot.m_stopERP = ((flags & SixDofFlags.BT_6DOF_FLAGS_ERP_STOP) != 0) ? m_linearLimits.m_stopERP[i] : info.erp; if (m_useOffsetForConstraintFrame) { int indx1 = (i + 1) % 3; int indx2 = (i + 2) % 3; bool rotAllowed = true; // rotations around orthos to current axis if (m_angularLimits[indx1].m_currentLimit != 0 && m_angularLimits[indx2].m_currentLimit != 0) { rotAllowed = false; } row += GetLimitMotorInfo2(limot, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB, info, row, ref axis, 0, rotAllowed); } else { row += GetLimitMotorInfo2(limot, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB, info, row, ref axis, 0, false); } } } return row; }
public static void PrintInfo2(StreamWriter writer, TypedConstraint constraint, ConstraintInfo2 info2) { if (writer != null) { writer.WriteLine(String.Format("getInfo2 [{0}] [{1}] [{2}] [{3}]", constraint.m_userConstraintId, constraint.GetObjectType(), (string)constraint.GetRigidBodyA().GetUserPointer(), (string)constraint.GetRigidBodyB().GetUserPointer())); writer.WriteLine(String.Format("numRows [{0}] fps[{1:0.00000000}] erp[{2:0.00000000}] findex[{3}] numIter[{4}]", info2.m_numRows, info2.fps, info2.erp, info2.findex, info2.m_numIterations)); for (int i = 0; i < info2.m_numRows; ++i) { writer.WriteLine(String.Format("TypedConstraint[{0}]", i)); writer.WriteLine("ContactNormal"); MathUtil.PrintVector3(writer, info2.m_solverConstraints[i].m_contactNormal); writer.WriteLine("rel1pos1CrossNormal"); MathUtil.PrintVector3(writer, info2.m_solverConstraints[i].m_relpos1CrossNormal); writer.WriteLine("rel1pos2CrossNormal"); MathUtil.PrintVector3(writer, info2.m_solverConstraints[i].m_relpos2CrossNormal); } } }
public override void GetInfo2 (ConstraintInfo2 info) { InternalUpdateSprings(info); base.GetInfo2(info); }
public static void PrintInfo2(StreamWriter writer, TypedConstraint constraint, ConstraintInfo2 info2) { if (writer != null) { writer.WriteLine(String.Format("getInfo2 [{0}] [{1}] [{2}] [{3}]", constraint.m_userConstraintId, constraint.GetObjectType(), (string)constraint.GetRigidBodyA().GetUserPointer(), (string)constraint.GetRigidBodyB().GetUserPointer())); writer.WriteLine(String.Format("numRows [{0}] fps[{1:0.00000000}] erp[{2:0.00000000}] findex[{3}] numIter[{4}]", info2.m_numRows, info2.fps, info2.erp, info2.findex, info2.m_numIterations)); for (int i = 0; i < info2.m_numRows; ++i) { writer.WriteLine(String.Format("TypedConstraint[{0}]", i)); writer.WriteLine("ContactNormal"); MathUtil.PrintVector3(writer, info2.m_solverConstraints[i].m_contactNormal); writer.WriteLine("rel1pos1CrossNormal"); MathUtil.PrintVector3(writer, info2.m_solverConstraints[i].m_relpos1CrossNormal); writer.WriteLine("rel1pos2CrossNormal"); MathUtil.PrintVector3(writer, info2.m_solverConstraints[i].m_relpos2CrossNormal); } } }
public override void GetInfo2(ConstraintInfo2 info) { InternalUpdateSprings(info); base.GetInfo2(info); }