// Loads the given OBJ file, creates a RigidBody with the given mess, and places
// at the origin of the ground.
protected void PrepareSimObj(string objFile, float mass, BulletSharp.Math.Vector3 inertia)
{
Debug.Log("Loading " + objFile + "...");
// Load wavefront file
OBJLoader.OBJMesh objloadermesh = OBJLoader.LoadOBJMesh(objFile);
Debug.Assert(objloadermesh.vertices.Count > 0);
Debug.Assert(objloadermesh.faces.Count > 0);
// Debug.Log("VERTS: " + objloadermesh.vertices.Count.ToString());
// Debug.Log("FACES: " + objloadermesh.faces.Count.ToString());
m_btmesh = DataGenUtils.BulletMeshFromUnity(objloadermesh);
Debug.Assert(m_btmesh.vertices.Length > 0);
Debug.Assert(m_btmesh.indices.Length > 0);
// Debug.Log("btVERTS: " + (btmesh.vertices.Length / 3).ToString());
// Debug.Log("btFACES: " + (btmesh.indices.Length / 3).ToString());
// Create a GImpactMeshShape for collider
var triVtxarray = new TriangleIndexVertexArray(m_btmesh.indices, m_btmesh.vertices);
m_cs = new GImpactMeshShape(triVtxarray);
m_cs.LocalScaling = new BulletSharp.Math.Vector3(1);
m_cs.Margin = bodyMargin;
m_cs.UpdateBound();
AddCollisionShape(m_cs);
// move it up so resting on the ground plane
float miny = float.MaxValue;
float cury;
for (int i = 0; i < objloadermesh.vertices.Count; i++)
{
cury = objloadermesh.vertices[i][1];
if (cury < miny)
{
miny = cury;
}
}
miny = -miny;
m_rbInitTransVec = new BulletSharp.Math.Vector3(0, miny + bodyMargin + m_groundMargin, 0);
m_rbInitTrans = Matrix.Translation(m_rbInitTransVec); // * Matrix.RotationY(Random.Range(0.0f, 360.0f));
m_rb = CreateRigidBody(mass, inertia, m_rbInitTrans, m_cs, bodyMat, bodyFriction, viz: RENDER_MODE);
m_rb.AngularFactor = angularFactor;
m_rb.SetSleepingThresholds(linearSleepThresh, angularSleepThresh);
if (DEBUG)
{
Debug.Log("LOADED MOMENT: " + m_rb.LocalInertia.ToString());
}
// if (DEBUG) Debug.Log("WORLD MOMENT: " + m_rb.InvInertiaTensorWorld.ToString());
// Debug.Log("Min y: " + (-miny).ToString());
if (DEBUG)
{
Debug.Log(m_rb.CenterOfMassPosition.ToString());
}
}
protected void SetupSim()
{
// randomly choose scaling before resetting rigid body
float randomMass = 0;
BulletSharp.Math.Vector3 randomInertia;
if (randomScale)
{
if (varyScale)
{
m_pclScale.x = Random.Range(this.scaleMin, this.scaleMax);
m_pclScale.y = Random.Range(this.scaleMin, this.scaleMax);
m_pclScale.z = Random.Range(this.scaleMin, this.scaleMax);
}
else
{
float uniformScale = Random.Range(this.scaleMin, this.scaleMax);
m_pclScale.x = uniformScale;
m_pclScale.y = uniformScale;
m_pclScale.z = uniformScale;
}
//// z can't be more than thrice or less than half of x scale
//float zmin = Mathf.Max(this.scaleMin, 0.5f * m_pclScale.x);
//float zmax = Mathf.Min(this.scaleMax, 3.0f * m_pclScale.x);
//randomScale = Random.Range(zmin, zmax);
//m_pclScale.z = randomScale;
//// y can't be greater than 2 times the smallest of x and z
//float ymax = 2.0f * Mathf.Min(m_pclScale.x, m_pclScale.z);
//randomScale = Random.Range(this.scaleMin, Mathf.Min(ymax, this.scaleMax));
//m_pclScale.y = randomScale;
if (DEBUG)
{
Debug.Log("Scaling by " + m_pclScale.ToString());
}
// randomMass = m_masses[m_curObjIdx] * m_pclScale.x * m_pclScale.y * m_pclScale.z;
float randomDensity;
if (useConstantDensity)
{
// density is constant so mass must scale with volume
randomDensity = densityMin;
randomMass = randomDensity * m_masses[m_curObjIdx] * m_pclScale.x * m_pclScale.y * m_pclScale.z;
}
else
{
randomDensity = Random.Range(densityMin, densityMax);
randomMass = randomDensity * m_masses[m_curObjIdx];
}
// inertia must scale with volume no matter if the density is constant or not
BulletSharp.Math.Vector3 objInertiaInfo = m_inertias[m_curObjIdx];
float scalexyz = m_pclScale.x * m_pclScale.y * m_pclScale.z;
float scalex2 = m_pclScale.x * m_pclScale.x;
float scaley2 = m_pclScale.y * m_pclScale.y;
float scalez2 = m_pclScale.z * m_pclScale.z;
float inertiax = randomDensity * scalexyz * (scaley2 * objInertiaInfo[1] + scalez2 * objInertiaInfo[2]);
float inertiay = randomDensity * scalexyz * (scalex2 * objInertiaInfo[0] + scalez2 * objInertiaInfo[2]);
float inertiaz = randomDensity * scalexyz * (scalex2 * objInertiaInfo[0] + scaley2 * objInertiaInfo[1]);
randomInertia = new BulletSharp.Math.Vector3(inertiax, inertiay, inertiaz);
// need to completely destory rigid body because need new mass/moment of inertia
DestroySimObj();
DataGenUtils.BulletOBJMesh scaledMesh = m_btmesh.Scale(m_pclScale.x, m_pclScale.y, m_pclScale.z);
var triVtxarray = new TriangleIndexVertexArray(scaledMesh.indices, scaledMesh.vertices);
m_cs = new GImpactMeshShape(triVtxarray);
m_cs.LocalScaling = new BulletSharp.Math.Vector3(1);
m_cs.Margin = bodyMargin;
m_cs.UpdateBound();
AddCollisionShape(m_cs);
// move it up so resting on the ground plane
float miny = float.MaxValue;
float maxz = float.MinValue;
float cury;
float curz;
for (int i = 0; i < scaledMesh.vertices.Length / 3; i++)
{
cury = scaledMesh.vertices[i * 3 + 1];
if (cury < miny)
{
miny = cury;
}
curz = scaledMesh.vertices[i * 3 + 2];
if (curz > maxz)
{
maxz = curz;
}
}
miny = -miny;
m_rbInitTransVec = new BulletSharp.Math.Vector3(0, miny + bodyMargin + m_groundMargin, 0);
m_rbInitTrans = Matrix.Translation(m_rbInitTransVec); //* Matrix.RotationY(Random.Range(0.0f, 360.0f));
//float gtInertiaX = (1.0f / 12.0f) * randomMass * (3.0f * maxz * maxz + (2.0f * miny) * (2.0f * miny));
//float gtInertiaZ = gtInertiaX;
//float gtInertiaY = 0.5f * randomMass * maxz * maxz;
//BulletSharp.Math.Vector3 gtInertia = new BulletSharp.Math.Vector3(gtInertiaX, gtInertiaY, gtInertiaZ);
//Debug.Log("GT INERTIA: " + gtInertia.ToString());
//randomInertia = gtInertia;
m_rb = CreateRigidBody(randomMass, randomInertia, m_rbInitTrans, m_cs, bodyMat, bodyFriction, viz: RENDER_MODE);
//m_rb = CreateRigidBody(randomMass, m_rbInitTrans, m_cs, bodyMat, bodyFriction);
m_rb.AngularFactor = angularFactor;
m_rb.SetSleepingThresholds(linearSleepThresh, angularSleepThresh);
m_mass = randomMass;
m_inertia = randomInertia;
m_density = randomDensity;
}
else
{
// using the same mesh just need to choose a new density
// steps for determinism
// https://pybullet.org/Bullet/phpBB3/viewtopic.php?t=3143
float randomDensity;
if (useConstantDensity)
{
randomDensity = densityMin;
}
else
{
randomDensity = Random.Range(densityMin, densityMax);
}
randomMass = randomDensity * m_masses[m_curObjIdx];
BulletSharp.Math.Vector3 objInertiaInfo = m_inertias[m_curObjIdx];
float inertiax = randomDensity * (objInertiaInfo[1] + objInertiaInfo[2]);
float inertiay = randomDensity * (objInertiaInfo[0] + objInertiaInfo[2]);
float inertiaz = randomDensity * (objInertiaInfo[0] + objInertiaInfo[1]);
randomInertia = new BulletSharp.Math.Vector3(inertiax, inertiay, inertiaz);
m_rb.SetMassProps(randomMass, randomInertia);
m_rbInitTrans = Matrix.Translation(m_rbInitTransVec); // * Matrix.RotationY(Random.Range(0.0f, 360.0f));
m_rb = ResetRigidBody(m_rb, randomMass, randomInertia, m_rbInitTrans, m_cs, bodyFriction);
m_rb.AngularFactor = angularFactor;
m_rb.SetSleepingThresholds(linearSleepThresh, angularSleepThresh);
// HingeConstraint hingeConstraint = new HingeConstraint(m_rb, new BulletSharp.Math.Vector3(0.0f), new BulletSharp.Math.Vector3(0.0f, 1.0f, 0.0f), false);
// m_world.AddConstraint(hingeConstraint);
// DestroySimObj(); // have to do this to set mass properties but can reuse previously calculated everything else
// if (m_cs == null) Debug.Log("NOT NULL");
// m_rb = CreateRigidBody(randomMass, randomInertia, m_rbInitTrans, m_cs, bodyMat, bodyFriction);
// m_rb.AngularFactor = new BulletSharp.Math.Vector3(angularFactor);
// m_rb.SetSleepingThresholds(linearSleepThresh, angularSleepThresh);
m_mass = randomMass;
m_inertia = randomInertia;
m_density = randomDensity;
}
m_stepCount = 0;
m_broadphase.ResetPool(m_colDispatcher);
m_solver.Reset();
float curMass = 1.0f / m_rb.InvMass;
if (DEBUG)
{
Debug.Log("Mass: " + curMass.ToString());
}
if (DEBUG)
{
Debug.Log("LOCAL MOMENT: " + m_rb.LocalInertia.ToString());
}
if (DEBUG)
{
Debug.Log("COM " + m_rb.CenterOfMassPosition.ToString());
}
if (DEBUG)
{
Debug.Log("Density " + m_density.ToString());
}
// determine impulse position
ClosestRayResultCallback cb;
BulletSharp.Math.Vector3 vertexNormal = new BulletSharp.Math.Vector3();
int missCount = 0;
do
{
// choose random vertex to apply force to
// pick a random point around in the plane around y position
float offsetx = UnityEngine.Random.Range(-100.0f, 100.0f);
float offsetz = UnityEngine.Random.Range(-100.0f, 100.0f);
Vector2 offsetvec = new Vector2(offsetx, offsetz);
// offsetvec.Normalize();
//float relForceHeight = 0.75f;
UnityEngine.Vector3 offsetPt = new UnityEngine.Vector3(offsetvec[0],
m_rb.CenterOfMassPosition.Y,
offsetvec[1]);
BulletSharp.Math.Vector3 btOffsetPt = BSExtensionMethods2.ToBullet(offsetPt);
BulletSharp.Math.Vector3 btInnerPt = m_rb.CenterOfMassPosition;
cb = new ClosestRayResultCallback(ref btOffsetPt, ref btInnerPt);
// Debug.DrawLine(BSExtensionMethods2.ToUnity(btInnerPt), offsetPt, Color.red, 2.0f);
m_world.RayTest(btOffsetPt, btInnerPt, cb);
if (cb.HasHit)
{
m_forcePoint = cb.HitPointWorld;
vertexNormal = cb.HitNormalWorld;
}
else
{
missCount++;
//Debug.Log("ERROR - couldn't find point to apply force to. Retrying...");
//return;
}
} while (!cb.HasHit);
if (DEBUG)
{
Debug.Log("Missed impulse " + missCount.ToString() + " times.");
}
if (DEBUG)
{
Debug.LogFormat("ForcePoint: " + m_forcePoint.ToString());
}
// get force vector
// loop until force is applied to outside of object
UnityEngine.Vector3 uForceVec = new UnityEngine.Vector3();
// initialize force vector to coincide with center of mass
BulletSharp.Math.Vector3 btForceVec = m_rb.CenterOfMassPosition - m_forcePoint;
// then randomly vary it within the x/z plane to be within the specified distance
BulletSharp.Math.Vector3 btVariationVec = new BulletSharp.Math.Vector3(-btForceVec[2], 0.0f, btForceVec[0]);
btVariationVec.Normalize();
float varyForce;
BulletSharp.Math.Vector3 proposedForceVec;
do
{
varyForce = UnityEngine.Random.Range(-forceDistMax, forceDistMax);
proposedForceVec = btVariationVec * varyForce + btForceVec;
} while (proposedForceVec.Dot(vertexNormal) >= 0); // must also be on the outside of the object
btForceVec = proposedForceVec;
btForceVec.Normalize();
uForceVec = BSExtensionMethods2.ToUnity(btForceVec);
if (DEBUG)
{
Debug.Log("FORCE DIST: " + varyForce.ToString());
}
//UnityEngine.Vector3 uVtxNormal = BSExtensionMethods2.ToUnity(vertexNormal);
//uVtxNormal.Normalize();
//do
//{
// float forcex = UnityEngine.Random.Range(-1.0f, 1.0f);
// float forcez = UnityEngine.Random.Range(-1.0f, 1.0f);
// uForceVec.Set(forcex, 0.0f, forcez);
// uForceVec.Normalize();
//} while (UnityEngine.Vector3.Dot(uForceVec, uVtxNormal) >= 0);
// random constrained magnitude
float mag = UnityEngine.Random.Range(impulseMin, impulseMax);
//Debug.Log("Vol: " + objectVolume.ToString());
// if (varyScale) {
// mag *= randomMass; // scale impulse t unity
//according to object scale
// } else {
// mag *= curMass;
// }
mag *= m_mass; // scale impulse according to object mass
uForceVec *= mag;
// set directly for debugging
//uForceVec.Set(2.5f, 0.0f, 0.0f);
//m_forcePoint = new BulletSharp.Math.Vector3(0.0f, m_rb.CenterOfMassPosition.Y, -0.15f);
m_forceVec = BSExtensionMethods2.ToBullet(uForceVec);
if (DEBUG)
{
Debug.LogFormat("ForceVec: " + m_forceVec.ToString());
}
if (DEBUG)
{
UnityEngine.Vector3 debugVec = -uForceVec;
debugVec.Scale(new UnityEngine.Vector3(0.5f, 0.5f, 0.5f));
Debug.DrawRay(BSExtensionMethods2.ToUnity(m_forcePoint), debugVec, Color.green, 1.0f);
Debug.DrawLine(BSExtensionMethods2.ToUnity(m_rb.CenterOfMassPosition), BSExtensionMethods2.ToUnity(m_forcePoint), Color.cyan, 1.0f);
Debug.DrawLine(BSExtensionMethods2.ToUnity(m_rb.CenterOfMassPosition), BSExtensionMethods2.ToUnity(m_rb.CenterOfMassPosition) + BSExtensionMethods2.ToUnity(btVariationVec) * varyForce, Color.blue, 1.0f);
}
// apply the random impulse
BulletSharp.Math.Vector3 radius = m_forcePoint - m_rb.CenterOfMassPosition;
m_rb.ApplyImpulse(m_forceVec, radius);
// m_rb.ApplyTorqueImpulse(new BulletSharp.Math.Vector3(0.0f, 1.0f, 0.0f));
// m_rb.ApplyCentralImpulse(new BulletSharp.Math.Vector3(4.0f, 0.0f, 2.0f));
// BulletSharp.Math.Vector3 newAngVel = m_rb.AngularVelocity;
// newAngVel.X = 0.0f;
// newAngVel.Z = 0.0f;
// m_rb.AngularVelocity = newAngVel;
// calculate ground truth for debugging
//BulletSharp.Math.Vector3 gtAngVel = radius.Cross(m_forceVec) / m_inertia;
//BulletSharp.Math.Vector3 gtLinVel = m_forceVec / m_mass;
//Debug.Log("GT LIN VEL: " + gtLinVel.ToString());
//Debug.Log("GT ANG VEL: " + gtAngVel.ToString());
}
