protected virtual int SetAngularLimits(ConstraintInfo2 info, int row_offset,ref Matrix transA,ref Matrix transB,ref Vector3 linVelA,ref Vector3 linVelB,ref Vector3 angVelA,ref Vector3 angVelB) { Generic6DofConstraint d6constraint = this; int row = row_offset; //solve angular limits for (int i=0;i<3 ;i++ ) { if(d6constraint.GetRotationalLimitMotor(i).NeedApplyTorques()) { Vector3 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; }
protected void InternalUpdateSprings(ConstraintInfo2 info) { // it is assumed that calculateTransforms() have been called before this call Vector3 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 = MathUtil.VectorComponent(ref 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; MathUtil.VectorComponent(ref m_linearLimits.m_targetVelocity,i,velFactor * force); MathUtil.VectorComponent(ref m_linearLimits.m_maxMotorForce,i,System.Math.Abs(force) / info.fps); } } for (int i = 0; i < 3; i++) { if (m_springEnabled[i + 3]) { // get current position of constraint float currPos = MathUtil.VectorComponent(ref 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 = System.Math.Abs(force) / info.fps; } } }
public virtual int GetLimitMotorInfo2(RotationalLimitMotor limot, ref Matrix transA,ref Matrix transB,ref Vector3 linVelA, ref Vector3 linVelB,ref Vector3 angVelA,ref Vector3 angVelB, ConstraintInfo2 info, int row, ref Vector3 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 //btScalar* J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis; //btScalar* 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) { Vector3 tmpA = Vector3.Zero, tmpB= Vector3.Zero, relA= Vector3.Zero, relB= Vector3.Zero; // get vector from bodyB to frameB in WCS relB = m_calculatedTransformB.Translation - transB.Translation; // get its projection to constraint axis Vector3 projB = ax1 * Vector3.Dot(relB,ax1); // get vector directed from bodyB to constraint axis (and orthogonal to it) Vector3 orthoB = relB - projB; // same for bodyA relA = m_calculatedTransformA.Translation - transA.Translation; Vector3 projA = ax1 * Vector3.Dot(relA,ax1); Vector3 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 Vector3 totalDist = projA + ax1 * desiredOffs - projB; // get offset vectors relA and relB relA = orthoA + totalDist * m_factA; relB = orthoB - totalDist * m_factB; tmpA = Vector3.Cross(relA,ax1); tmpB = Vector3.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 { Vector3 ltd; // Linear Torque Decoupling vector Vector3 c = m_calculatedTransformB.Translation - transA.Translation; ltd = Vector3.Cross(c,ax1); info.m_solverConstraints[row].m_relpos1CrossNormal = ltd; MathUtil.ZeroCheckVector(info.m_solverConstraints[row].m_relpos1CrossNormal); c = m_calculatedTransformB.Translation - transB.Translation; ltd = -Vector3.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 = Vector3.Dot(angVelA,ax1); vel -= Vector3.Dot(angVelB,ax1); } else { vel = Vector3.Dot(linVelA,ax1); vel -= Vector3.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,Matrix transA,Matrix transB,Vector3 linVelA,Vector3 linVelB,Vector3 angVelA,Vector3 angVelB) { //prepare constraint CalculateTransforms(ref transA, ref transB); 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 GetInfo2InternalUsingFrameOffset(ConstraintInfo2 info, ref Matrix transA, ref Matrix transB, ref Vector3 angVelA, ref Vector3 angVelB) { // transforms in world space if (BulletGlobals.g_streamWriter != null && debugHingeConstrainst) { 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); } Matrix trA = MathUtil.BulletMatrixMultiply(transA, m_rbAFrame); Matrix trB = MathUtil.BulletMatrixMultiply(transB, m_rbBFrame); if (BulletGlobals.g_streamWriter != null && debugHingeConstrainst) { MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "trA", trA); MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "trB", trB); } // pivot point Vector3 pivotAInW = trA.Translation; Vector3 pivotBInW = trB.Translation; #if true // difference between frames in WCS Vector3 ofs = trB.Translation - trA.Translation; // 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 Vector3 ax1A = MathUtil.MatrixColumn(ref trA,2); Vector3 ax1B = MathUtil.MatrixColumn(ref trB,2); Vector3 ax1 = ax1A * factA + ax1B * factB; ax1.Normalize(); // fill first 3 rows // we want: velA + wA x relA == velB + wB x relB Matrix bodyA_trans = transA; Matrix bodyB_trans = transB; int s0 = 0; int s1 = 1; int s2 = 2; int nrow = 2; // last filled row Vector3 tmpA, tmpB, relA, relB, p, q; // get vector from bodyB to frameB in WCS relB = trB.Translation - bodyB_trans.Translation; // get its projection to hinge axis Vector3 projB = ax1 * Vector3.Dot(relB,ax1); // get vector directed from bodyB to hinge axis (and orthogonal to it) Vector3 orthoB = relB - projB; // same for bodyA relA = trA.Translation - bodyA_trans.Translation; Vector3 projA = ax1 * Vector3.Dot(relA,ax1); Vector3 orthoA = relA - projA; Vector3 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 = MathUtil.MatrixColumn(ref trA,1); } // make one more ortho q = Vector3.Cross(ax1,p); // fill three rows tmpA = Vector3.Cross(relA,p); tmpB = Vector3.Cross(relB,p); info.m_solverConstraints[s0].m_relpos1CrossNormal = tmpA; info.m_solverConstraints[s0].m_relpos2CrossNormal = -tmpB; tmpA = Vector3.Cross(relA,q); tmpB = Vector3.Cross(relB,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 = Vector3.Cross(relA,ax1); tmpB = Vector3.Cross(relB,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; 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 k = info.fps * info.erp; float rhs = k * Vector3.Dot(p,ofs); info.m_solverConstraints[s0].m_rhs = rhs; rhs = k * Vector3.Dot(q,ofs); info.m_solverConstraints[s1].m_rhs = rhs; rhs = k * Vector3.Dot(ax1,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; Vector3 u = Vector3.Cross(ax1A,ax1B); info.m_solverConstraints[s3].m_rhs = k * Vector3.Dot(u,p); info.m_solverConstraints[s4].m_rhs = k * Vector3.Dot(u,q); #endif // check angular limits nrow = 4; // last filled row int srow; float limit_err = 0f; int limit = 0; if(GetSolveLimit()) { limit_err = m_correction * m_referenceSign; 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) float bounce = m_relaxationFactor; if(bounce > 0f) { float vel = Vector3.Dot(angVelA,ax1); vel -= Vector3.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[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; } } } } info.m_solverConstraints[srow].m_rhs *= m_biasFactor; } // if(limit) } // if angular limit or powered if (BulletGlobals.g_streamWriter != null && debugHingeConstrainst) { PrintInfo2(BulletGlobals.g_streamWriter, this, info); } }
public void GetInfo2InternalUsingFrameOffset(ConstraintInfo2 info, Matrix transA, Matrix transB, Vector3 angVelA, Vector3 angVelB) { GetInfo2InternalUsingFrameOffset(info, ref transA, ref transB, ref angVelA, ref angVelB); }
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_solverConstraints.Length,info2.fps,info2.erp,info2.findex,info2.m_numIterations)); for(int i=0;i<info2.m_solverConstraints.Length;++i) { writer.WriteLine(String.Format("SolverConstaint[{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 void GetInfo2NonVirtual(ConstraintInfo2 info,Matrix body0_trans,Matrix 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; Vector3 a1 = Vector3.TransformNormal(GetPivotInA(),body0_trans); { Vector3 angular0 = info.m_solverConstraints[0].m_relpos1CrossNormal; Vector3 angular1 = info.m_solverConstraints[1].m_relpos1CrossNormal; Vector3 angular2 = info.m_solverConstraints[2].m_relpos1CrossNormal; Vector3 a1neg = -a1; MathUtil.GetSkewSymmetricMatrix(ref a1neg, ref angular0, ref angular1, ref angular2); } /*info->m_J2linearAxis[0] = -1; info->m_J2linearAxis[s+1] = -1; info->m_J2linearAxis[2*s+2] = -1; */ Vector3 a2 = Vector3.TransformNormal(GetPivotInB(),body1_trans); { Vector3 a2n = -a2; Vector3 angular0 = info.m_solverConstraints[0].m_relpos2CrossNormal; Vector3 angular1 = info.m_solverConstraints[1].m_relpos2CrossNormal; Vector3 angular2 = info.m_solverConstraints[2].m_relpos2CrossNormal; MathUtil.GetSkewSymmetricMatrix(ref a2, ref angular0, ref angular1, ref angular2); } // 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; Vector3 body0Origin = body0_trans.Translation; Vector3 body1Origin = body1_trans.Translation; for (j = 0; j < 3; j++) { info.m_solverConstraints[j].m_rhs = k * (MathUtil.VectorComponent(ref a2,j) + MathUtil.VectorComponent(ref body1Origin,j) - MathUtil.VectorComponent(ref a1,j) - MathUtil.VectorComponent(ref 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; } } }
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 GetInfo2NonVirtual(ConstraintInfo2 info, Matrix transA, Matrix transB, Matrix invInertiaWorldA, Matrix invInertiaWorldB) { CalcAngleInfo2(ref transA, ref transB, ref invInertiaWorldA, ref invInertiaWorldB); // 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; Vector3 a1 = Vector3.TransformNormal(m_rbAFrame.Translation, transA); { Vector3 a1neg = -a1; MathUtil.GetSkewSymmetricMatrix(ref a1neg, ref info.m_solverConstraints[0].m_relpos1CrossNormal, ref info.m_solverConstraints[1].m_relpos1CrossNormal, ref info.m_solverConstraints[2].m_relpos1CrossNormal); } Vector3 a2 = Vector3.TransformNormal(m_rbBFrame.Translation, transB); { MathUtil.GetSkewSymmetricMatrix(ref a2, ref info.m_solverConstraints[0].m_relpos2CrossNormal, ref info.m_solverConstraints[1].m_relpos2CrossNormal, ref 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 * (MathUtil.VectorComponent(ref a2, j) + MathUtil.VectorComponent(transB.Translation, j) - MathUtil.VectorComponent(ref a1, j) - MathUtil.VectorComponent(transA.Translation, 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; Vector3 ax1; // angular limits if (m_solveSwingLimit) { if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) { Matrix trA = MathUtil.BulletMatrixMultiply(transA, m_rbAFrame); Vector3 p = MathUtil.MatrixColumn(ref trA, 1); Vector3 q = MathUtil.MatrixColumn(ref trA, 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 * Vector3.Dot(m_swingAxis, p); info.m_solverConstraints[row + 1].m_rhs = fact * Vector3.Dot(m_swingAxis, 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 && debugConstraint) { PrintInfo2(BulletGlobals.g_streamWriter, this, info); } }
public override void GetInfo2 (ConstraintInfo2 info) { InternalUpdateSprings(info); base.GetInfo2(info); }
public void TestLinLimits2(ConstraintInfo2 info) { }
public void GetInfo2NonVirtual(ConstraintInfo2 info, Matrix transA, Matrix transB, Vector3 linVelA, Vector3 linVelB, float rbAinvMass, float rbBinvMass) { Matrix trA = GetCalculatedTransformA(); Matrix trB = GetCalculatedTransformB(); Debug.Assert(!m_useSolveConstraintObsolete); int i, s = 1; float signFact = m_useLinearReferenceFrameA ? 1.0f : -1.0f; // difference between frames in WCS Vector3 ofs = trB.Translation - trA.Translation; // 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; Vector3 ax1 = Vector3.Zero, p= Vector3.Zero, q= Vector3.Zero; Vector3 ax1A = MathUtil.MatrixColumn(ref trA,0); Vector3 ax1B = MathUtil.MatrixColumn(ref trB,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, ref p, ref q); } else { // old way - use frameA ax1 = MathUtil.MatrixColumn(ref trA,0); // get 2 orthos to slider axis (Y, Z) p = MathUtil.MatrixColumn(ref trA,1); q = MathUtil.MatrixColumn(ref trA,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; Vector3 u = Vector3.Cross(ax1A,ax1B); info.m_solverConstraints[0].m_rhs = k * Vector3.Dot(u,p); info.m_solverConstraints[s].m_rhs = k * Vector3.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 Matrix bodyA_trans = transA; Matrix bodyB_trans = transB; nrow++; int s2 = nrow * s; nrow++; int s3 = nrow * s; Vector3 tmpA = Vector3.Zero, tmpB = Vector3.Zero, relA = Vector3.Zero, relB = Vector3.Zero, c = Vector3.Zero; if(m_useOffsetForConstraintFrame) { // get vector from bodyB to frameB in WCS relB = trB.Translation - bodyB_trans.Translation; // get its projection to slider axis Vector3 projB = ax1 * Vector3.Dot(relB,ax1); // get vector directed from bodyB to slider axis (and orthogonal to it) Vector3 orthoB = relB - projB; // same for bodyA relA = trA.Translation - bodyA_trans.Translation; Vector3 projA = ax1 * Vector3.Dot(relA,ax1); Vector3 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 Vector3 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 = MathUtil.MatrixColumn(ref trA,1); } // make one more ortho q = Vector3.Cross(ax1,p); // fill two rows tmpA = Vector3.Cross(relA,p); tmpB = Vector3.Cross(relB,p); info.m_solverConstraints[s2].m_relpos1CrossNormal = tmpA; info.m_solverConstraints[s2].m_relpos2CrossNormal = -tmpB; tmpA = Vector3.Cross(relA,q); tmpB = Vector3.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 Vector3 tmp = Vector3.Cross(c, p); info.m_solverConstraints[s2].m_relpos1CrossNormal = factA * tmp; info.m_solverConstraints[s2].m_relpos2CrossNormal = factB * tmp; tmp = Vector3.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 * Vector3.Dot(p,ofs); info.m_solverConstraints[s2].m_rhs = rhs; rhs = k * Vector3.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 = Vector3.Cross(relA,ax1); tmpB = Vector3.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 Vector3 ltd = Vector3.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 = System.Math.Abs(1.0f - GetDampingLimLin()); if(bounce > 0.0f) { float vel = Vector3.Dot(linVelA,ax1); vel -= Vector3.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 = System.Math.Abs(1.0f - GetDampingLimAng()); if (bounce > 0.0f) { float vel = Vector3.Dot(m_rbA.GetAngularVelocity(), ax1); vel -= Vector3.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 && debugConstraint) { PrintInfo2(BulletGlobals.g_streamWriter, this, info); } }
//protected float solveSingleIteration(int iteration, ObjectArray<CollisionObject> bodies, int numBodies, ObjectArray<PersistentManifold> manifold, int numManifolds, ObjectArray<TypedConstraint> constraints, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer, IDispatcher dispatcher) //protected float solveSingleIteration(int iteration, ObjectArray<CollisionObject> bodies, int numBodies, ObjectArray<PersistentManifold> manifold, int numManifolds, ObjectArray<TypedConstraint> constraints, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer) //{ // return 0f; //} protected virtual float SolveGroupCacheFriendlySetup(ObjectArray<CollisionObject> bodies, int numBodies, ObjectArray<PersistentManifold> manifold, int numManifolds, ObjectArray<TypedConstraint> constraints, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer, IDispatcher dispatcher) { //BT_PROFILE("solveGroupCacheFriendlySetup"); m_counter++; if ((numConstraints + numManifolds) == 0) { // printf("empty\n"); return 0f; } //if (true) //{ // for (int j=0;j<numConstraints;j++) // { // TypedConstraint constraint = constraints[j]; // constraint.buildJacobian(); // } //} //btRigidBody* rb0=0,*rb1=0; //if (1) { { int totalNumRows = 0; //calculate the total number of contraint rows m_tmpConstraintSizesPool.Clear(); for (int i = 0; i < numConstraints; i++) { ConstraintInfo1 info1 = new ConstraintInfo1(); m_tmpConstraintSizesPool.Add(info1); constraints[i].GetInfo1(info1); totalNumRows += info1.m_numConstraintRows; } ResizeSolverConstraintList(m_tmpSolverNonContactConstraintPool, totalNumRows); ///setup the btSolverConstraints int currentRow = 0; for (int i = 0; i < numConstraints; i++) { ConstraintInfo1 info1a = m_tmpConstraintSizesPool[i]; if (info1a.m_numConstraintRows != 0) { Debug.Assert(currentRow < totalNumRows); //SolverConstraint currentConstraintRow = m_tmpSolverNonContactConstraintPool[currentRow]; TypedConstraint constraint = constraints[i]; RigidBody rbA = constraint.GetRigidBodyA(); RigidBody rbB = constraint.GetRigidBodyB(); for (int j = currentRow; j < (currentRow + info1a.m_numConstraintRows); j++) { SolverConstraint solverConstraint = new SolverConstraint(); solverConstraint.m_lowerLimit = float.MinValue; solverConstraint.m_upperLimit = float.MaxValue; solverConstraint.m_appliedImpulse = 0f; solverConstraint.m_appliedPushImpulse = 0f; solverConstraint.m_solverBodyA = rbA; solverConstraint.m_solverBodyB = rbB; m_tmpSolverNonContactConstraintPool[j] = solverConstraint; } Vector3 zero = Vector3.Zero; rbA.InternalSetDeltaLinearVelocity(ref zero); rbA.InternalSetDeltaAngularVelocity(ref zero); rbB.InternalSetDeltaLinearVelocity(ref zero); rbB.InternalSetDeltaAngularVelocity(ref zero); ConstraintInfo2 info2 = new ConstraintInfo2(); info2.m_solverConstraints = new SolverConstraint[info1a.m_numConstraintRows]; // MAN - copy the data into the info block for passing to the constraints for (int j = 0; j < info1a.m_numConstraintRows; ++j) { info2.m_solverConstraints[j] = m_tmpSolverNonContactConstraintPool[currentRow + j]; } info2.fps = 1f / infoGlobal.m_timeStep; info2.erp = infoGlobal.m_erp; info2.m_numIterations = infoGlobal.m_numIterations; constraints[i].GetInfo2(info2); ///finalize the constraint setup /// for (int j = 0; j < info1a.m_numConstraintRows; ++j) { m_tmpSolverNonContactConstraintPool[currentRow + j] = info2.m_solverConstraints[j]; } //FIXME - log the output of the solverconstraints for comparison. for (int j = 0; j < (info1a.m_numConstraintRows); j++) { SolverConstraint solverConstraint = m_tmpSolverNonContactConstraintPool[currentRow + j]; solverConstraint.m_originalContactPoint = constraint; { Vector3 ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal; solverConstraint.m_angularComponentA = Vector3.TransformNormal(ftorqueAxis1, constraint.GetRigidBodyA().GetInvInertiaTensorWorld()) * constraint.GetRigidBodyA().GetAngularFactor(); } { Vector3 ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; solverConstraint.m_angularComponentB = Vector3.TransformNormal(ftorqueAxis2, constraint.GetRigidBodyB().GetInvInertiaTensorWorld()) * constraint.GetRigidBodyB().GetAngularFactor(); } { Vector3 iMJlA = solverConstraint.m_contactNormal * rbA.GetInvMass(); Vector3 iMJaA = Vector3.TransformNormal(solverConstraint.m_relpos1CrossNormal, rbA.GetInvInertiaTensorWorld()); Vector3 iMJlB = solverConstraint.m_contactNormal * rbB.GetInvMass();//sign of normal? Vector3 iMJaB = Vector3.TransformNormal(solverConstraint.m_relpos2CrossNormal, rbB.GetInvInertiaTensorWorld()); float sum = Vector3.Dot(iMJlA, solverConstraint.m_contactNormal); float a = Vector3.Dot(iMJaA, solverConstraint.m_relpos1CrossNormal); float b = Vector3.Dot(iMJlB, solverConstraint.m_contactNormal); float c = Vector3.Dot(iMJaB, solverConstraint.m_relpos2CrossNormal); sum += a; sum += b; sum += c; solverConstraint.m_jacDiagABInv = 1f / sum; MathUtil.SanityCheckFloat(solverConstraint.m_jacDiagABInv); } ///fix rhs ///todo: add force/torque accelerators { float rel_vel; float vel1Dotn = Vector3.Dot(solverConstraint.m_contactNormal, rbA.GetLinearVelocity()) + Vector3.Dot(solverConstraint.m_relpos1CrossNormal, rbA.GetAngularVelocity()); float vel2Dotn = -Vector3.Dot(solverConstraint.m_contactNormal, rbB.GetLinearVelocity()) + Vector3.Dot(solverConstraint.m_relpos2CrossNormal, rbB.GetAngularVelocity()); rel_vel = vel1Dotn + vel2Dotn; float restitution = 0f; float positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2 float velocityError = restitution - rel_vel;// * damping; float penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; float velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; solverConstraint.m_appliedImpulse = 0f; } if (BulletGlobals.g_streamWriter != null && debugSolver) { TypedConstraint.PrintSolverConstraint(BulletGlobals.g_streamWriter, solverConstraint, j); } m_tmpSolverNonContactConstraintPool[currentRow + j] = solverConstraint; } } currentRow += m_tmpConstraintSizesPool[i].m_numConstraintRows; } } { PersistentManifold manifold2 = null; CollisionObject colObj0 = null, colObj1 = null; for (int i = 0; i < numManifolds; i++) { manifold2 = manifold[i]; ConvertContact(manifold2, infoGlobal); } } } ContactSolverInfo info = infoGlobal; int numConstraintPool = m_tmpSolverContactConstraintPool.Count; int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.Count; ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints //m_orderTmpConstraintPool.Capacity = numConstraintPool; //m_orderFrictionConstraintPool.Capacity = numFrictionPool; m_orderTmpConstraintPool.Clear(); m_orderFrictionConstraintPool.Clear(); { for (int i = 0; i < numConstraintPool; i++) { m_orderTmpConstraintPool.Add(i); } for (int i = 0; i < numFrictionPool; i++) { m_orderFrictionConstraintPool.Add(i); } } return 0f; }
protected virtual int SetLinearLimits(ConstraintInfo2 info, int row, ref Matrix transA,ref Matrix transB,ref Vector3 linVelA,ref Vector3 linVelB,ref Vector3 angVelA,ref Vector3 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 = MathUtil.VectorComponent(ref m_linearLimits.m_currentLinearDiff,i); limot.m_currentLimitError = MathUtil.VectorComponent(ref m_linearLimits.m_currentLimitError,i); limot.m_damping = m_linearLimits.m_damping; limot.m_enableMotor = m_linearLimits.m_enableMotor[i]; limot.m_hiLimit = MathUtil.VectorComponent(m_linearLimits.m_upperLimit,i); limot.m_limitSoftness = m_linearLimits.m_limitSoftness; limot.m_loLimit = MathUtil.VectorComponent(m_linearLimits.m_lowerLimit,i); limot.m_maxLimitForce = 0f; limot.m_maxMotorForce = MathUtil.VectorComponent(m_linearLimits.m_maxMotorForce,i); limot.m_targetVelocity = MathUtil.VectorComponent(m_linearLimits.m_targetVelocity,i); Vector3 axis = MathUtil.MatrixColumn(m_calculatedTransformA,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) ? MathUtil.VectorComponent(ref m_linearLimits.m_normalCFM,i) : info.m_solverConstraints[0].m_cfm; limot.m_stopCFM = ((flags & SixDofFlags.BT_6DOF_FLAGS_CFM_STOP) != 0) ? MathUtil.VectorComponent(ref m_linearLimits.m_stopCFM,i) : info.m_solverConstraints[0].m_cfm; limot.m_stopERP = ((flags & SixDofFlags.BT_6DOF_FLAGS_ERP_STOP) != 0) ? MathUtil.VectorComponent(ref 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 void GetInfo2NonVirtual(ConstraintInfo2 info,Matrix transA,Matrix transB,Vector3 angVelA,Vector3 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 override void GetInfo2(ConstraintInfo2 info) { GetInfo2NonVirtual(info, m_rbA.GetCenterOfMassTransform(), m_rbB.GetCenterOfMassTransform()); }
public void GetInfo2Internal(ConstraintInfo2 info,Matrix transA,Matrix transB,Vector3 angVelA,Vector3 angVelB) { // transforms in world space Matrix trA = MathUtil.BulletMatrixMultiply(ref transA,ref m_rbAFrame); Matrix trB = MathUtil.BulletMatrixMultiply(ref transB,ref m_rbBFrame); // pivot point Vector3 pivotAInW = trA.Translation; Vector3 pivotBInW = trB.Translation; // linear (all fixed) //info.m_J1linearAxis[0] = 1; //info.m_J1linearAxis[s + 1] = 1; //info.m_J1linearAxis[2 * s + 2] = 1; info.m_solverConstraints[0].m_contactNormal.X = 1f; info.m_solverConstraints[1].m_contactNormal.Y = 1f; info.m_solverConstraints[2].m_contactNormal.Z = 1f; Vector3 a1 = pivotAInW - transA.Translation; { Vector3 a1neg = -a1; MathUtil.GetSkewSymmetricMatrix(ref a1neg, ref info.m_solverConstraints[0].m_relpos1CrossNormal, ref info.m_solverConstraints[1].m_relpos1CrossNormal, ref info.m_solverConstraints[2].m_relpos1CrossNormal); //if (info.m_solverConstraints[0].m_relpos1CrossNormal.X == 0.15) //{ // int ibreak = 0; //} int ibreak = 0; } Vector3 a2 = pivotBInW - transB.Translation; { MathUtil.GetSkewSymmetricMatrix(ref a2, ref info.m_solverConstraints[0].m_relpos2CrossNormal, ref info.m_solverConstraints[1].m_relpos2CrossNormal, ref info.m_solverConstraints[2].m_relpos2CrossNormal); } // linear RHS float k = info.fps * info.erp; for (int i = 0; i < 3; i++) { float val = k * (MathUtil.VectorComponent(ref pivotBInW,i) - MathUtil.VectorComponent(ref 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) Vector3 ax1 = MathUtil.MatrixColumn(ref trA,2); // get 2 orthos to hinge axis (X, Y) Vector3 p = MathUtil.MatrixColumn(ref trA,0); Vector3 q = MathUtil.MatrixColumn(ref trA,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. Vector3 ax2 = MathUtil.MatrixColumn(ref trB,2); Vector3 u = Vector3.Cross(ax1,ax2); info.m_solverConstraints[s3].m_rhs = k * Vector3.Dot(u, p); info.m_solverConstraints[s4].m_rhs = k * Vector3.Dot(u, q); // check angular limits int nrow = 4; // last filled row float limit_err = 0.0f; int limit = 0; if (GetSolveLimit()) { limit_err = m_correction * m_referenceSign; limit = (limit_err >0.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++; 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 * info.erp); 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) float bounce = m_relaxationFactor; if (bounce > 0f) { float vel = Vector3.Dot(angVelA,ax1); vel -= Vector3.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; } } } } info.m_solverConstraints[nrow].m_rhs *= m_biasFactor; } // if(limit) } // if angular limit or powered if (BulletGlobals.g_streamWriter != null && debugHingeConstrainst) { PrintInfo2(BulletGlobals.g_streamWriter, this, info); } }
public override void GetInfo2(ConstraintInfo2 info) { }
public virtual void GetInfo2(ConstraintInfo2 info) { }