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)
{
}
