public override void GetInfo1(ConstraintInfo1 info) { }
public override void GetInfo1(ConstraintInfo1 info) { GetInfo1NonVirtual(info); }
public void GetInfo1NonVirtual(ConstraintInfo1 info) { info.m_numConstraintRows = 3; info.nub = 3; }
public override void GetInfo1(ConstraintInfo1 info) { //prepare constraint CalculateTransforms(m_rbA.GetCenterOfMassTransform(),m_rbB.GetCenterOfMassTransform()); info.m_numConstraintRows = 0; info.nub = 6; int i; //test linear limits for (i = 0; i < 3; i++) { if (m_linearLimits.NeedApplyForce(i)) { info.m_numConstraintRows++; info.nub--; } } //test angular limits for (i = 0; i < 3; i++) { if (TestAngularLimitMotor(i)) { info.m_numConstraintRows++; info.nub--; } } }
public void GetInfo1NonVirtual(ConstraintInfo1 info) { //pre-allocate all 6 info.m_numConstraintRows = 6; info.nub = 0; }
public override void GetInfo1(ConstraintInfo1 info) { info.m_numConstraintRows = 5; // Fixed 3 linear + 2 angular info.nub = 1; //prepare constraint TestLimit(m_rbA.GetCenterOfMassTransform(),m_rbB.GetCenterOfMassTransform()); if (GetSolveLimit() || GetEnableAngularMotor()) { info.m_numConstraintRows++; // limit 3rd anguar as well info.nub--; } if (BulletGlobals.g_streamWriter != null && debugConstraint) { PrintInfo1(BulletGlobals.g_streamWriter,this,info); } }
public void GetInfo1NonVirtual(ConstraintInfo1 info) { //always add the 'limit' row, to avoid computation (data is not available yet) info.m_numConstraintRows = 6; // Fixed 3 linear + 2 angular info.nub = 0; if (BulletGlobals.g_streamWriter != null && debugConstraint) { PrintInfo1(BulletGlobals.g_streamWriter, this, info); } }
public void GetInfo1NonVirtual(ConstraintInfo1 info) { //always reserve 6 rows: object transform is not available on SPU info.m_numConstraintRows = 6; info.nub = 0; }
public override void GetInfo1(ConstraintInfo1 info) { info.m_numConstraintRows = 3; info.nub = 3; if(BulletGlobals.g_streamWriter != null && debugConstraint) { PrintInfo1(BulletGlobals.g_streamWriter, this, info); } CalcAngleInfo2(m_rbA.GetCenterOfMassTransform(), m_rbB.GetCenterOfMassTransform(), m_rbA.GetInvInertiaTensorWorld(), m_rbB.GetInvInertiaTensorWorld()); if (m_solveSwingLimit) { info.m_numConstraintRows++; info.nub--; if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) { info.m_numConstraintRows++; info.nub--; } } if (m_solveTwistLimit) { info.m_numConstraintRows++; info.nub--; } }
public void GetInfo1NonVirtual(ConstraintInfo1 info) { info.m_numConstraintRows = 6; // Fixed 2 linear + 2 angular + 1 limit (even if not used) info.nub = 0; }
public override void GetInfo1(ConstraintInfo1 info) { info.m_numConstraintRows = 4; // Fixed 2 linear + 2 angular info.nub = 2; //prepare constraint CalculateTransforms(m_rbA.GetCenterOfMassTransform(), m_rbB.GetCenterOfMassTransform()); TestAngLimits(); TestLinLimits(); if (GetSolveLinLimit() || GetPoweredLinMotor()) { info.m_numConstraintRows++; // limit 3rd linear as well info.nub--; } //testAngLimits(); if (GetSolveAngLimit() || GetPoweredAngMotor()) { info.m_numConstraintRows++; // limit 3rd angular as well info.nub--; } if (BulletGlobals.g_streamWriter != null && debugConstraint) { PrintInfo1(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; }
public static void PrintInfo1(StreamWriter writer,TypedConstraint constraint,ConstraintInfo1 info) { if (writer != null) { writer.WriteLine("getInfo1 [{0}] [{1}] [{2}] [{3}]", constraint.m_userConstraintId, constraint.GetObjectType(), (string)constraint.GetRigidBodyA().GetUserPointer(), (string)constraint.GetRigidBodyB().GetUserPointer()); MathUtil.PrintMatrix(writer, "rBA cmot", constraint.GetRigidBodyA().GetCenterOfMassTransform()); MathUtil.PrintMatrix(writer, "rBB cmot", constraint.GetRigidBodyB().GetCenterOfMassTransform()); MathUtil.PrintMatrix(writer, "rBA inv tensor", constraint.GetRigidBodyA().GetInvInertiaTensorWorld()); MathUtil.PrintMatrix(writer, "rBB inv tensor", constraint.GetRigidBodyB().GetInvInertiaTensorWorld()); writer.WriteLine(String.Format("NumRows [{0}] Nub[{1}]", info.m_numConstraintRows, info.nub)); } }
public virtual void GetInfo1(ConstraintInfo1 info) { }