// The save state functions save various information about the rigid
// body simulation to global variables to be written out.
protected void SaveInitState()
{
m_com0 = m_rb.CenterOfMassPosition;
Matrix curTrans = m_rb.WorldTransform;
m_pos0 = BSExtensionMethods2.ExtractTranslationFromMatrix(ref curTrans);
m_vel0 = m_rb.LinearVelocity;
m_angvel0 = m_rb.AngularVelocity;
if (DEBUG)
{
Debug.Log("Init ANGVEL: " + m_angvel0.ToString());
}
if (DEBUG)
{
Debug.Log("Init ANGVEL len: " + (m_angvel0.Length * Time.fixedDeltaTime).ToString());
}
m_rot0 = BSExtensionMethods2.ExtractRotationFromMatrix(ref curTrans);
m_eulerrot0 = m_rot0.eulerAngles;
}
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());
}
// Called after each simulation step to perform any necessary updates.
protected override void UpdateSim()
{
// Debug.Log ("UpdateSim");
//if (DEBUG) Debug.DrawRay(BSExtensionMethods2.ToUnity(m_rb.CenterOfMassPosition), BSExtensionMethods2.ToUnity(m_rb.LinearVelocity), Color.red, 20.0f);
if (DEBUG)
{
Debug.DrawRay(BSExtensionMethods2.ToUnity(m_rb.CenterOfMassPosition), (BSExtensionMethods2.ToUnity(m_rb.LinearVelocity) / m_rb.LinearVelocity.Length) * 0.01f, Color.red, 20.0f);
}
//if (m_stepCount == 0) {
// Debug.Log("INIT LINVEL2: " + m_rb.LinearVelocity.ToString());
// Debug.Log("INIT ANGVEL2: " + m_rb.AngularVelocity.ToString());
//}
// update rotation tracker
BulletSharp.Math.Vector3 angleRotated = Time.fixedDeltaTime * (Mathf.Rad2Deg * m_rb.AngularVelocity);
//if ((m_totalRot[1] > 0 && angleRotated[1] < 0) || (m_totalRot[1] < 0 && angleRotated[1] > 0)) {
// Debug.Log("SWAP: " + angleRotated[1].ToString());
//}
m_totalRot += angleRotated;
// check if the current rigid body is still moving (isactive)
if (!m_rb.IsActive)
{
if (DEBUG)
{
Debug.Log("TOTAL ROT: " + m_totalRot.ToString());
}
// record the current state and dump all info to file
SaveCurrentState();
SaveFinalState();
DumpSimInfo();
// check if we're done with this object;
if (++m_curSimNum >= numSimsPerObject)
{
if (++m_curObjIdx >= m_objFiles.Length)
{
Debug.Log("BAD FILES: " + m_fallenFiles.Count.ToString() + ", NUM STUCK: " + m_stuckCount.ToString());
for (int k = 0; k < m_fallenFiles.Count; k++)
{
Debug.Log(m_fallenFiles[k]);
}
DumpFallenObjects();
ExitSimulation();
}
else
{
DestroySimObj();
// create new directory to store sims from the next object
m_curDataOutDir = Path.Combine(dataOutDir, m_objFiles[m_curObjIdx].Replace(".obj", "") + "/");
Directory.CreateDirectory(m_curDataOutDir);
PrepareSimObj(m_baseDataDir + m_objFiles[m_curObjIdx], m_masses[m_curObjIdx], m_inertias[m_curObjIdx]);
m_curVideoOutDir = Path.Combine(Application.dataPath, videoOutDir);
m_curVideoOutDir = Path.Combine(m_curVideoOutDir, m_objFiles[m_curObjIdx].Replace(".obj", "") + "/");
Directory.CreateDirectory(m_curVideoOutDir);
string curSimOut = Path.Combine(m_curVideoOutDir, "sim_0");
if (!Directory.Exists(curSimOut))
{
Directory.CreateDirectory(curSimOut);
}
m_renderFrame = 1;
// ready the first sim
SetupSim();
SaveInitState();
ClearStateLists();
m_curSimNum = 0;
// reset rotation tracking
m_totalRot = new BulletSharp.Math.Vector3(0.0f, 0.0f, 0.0f);
m_numPerturbs = 0;
}
m_fallCount = 0;
}
else
{
// set up next sim with the same object
SetupSim();
SaveInitState();
ClearStateLists();
// reset rotation tracking
m_totalRot = new BulletSharp.Math.Vector3(0.0f, 0.0f, 0.0f);
m_numPerturbs = 0;
string curSimOut = Path.Combine(m_curVideoOutDir, "sim_" + m_curSimNum.ToString());
if (!Directory.Exists(curSimOut))
{
Directory.CreateDirectory(curSimOut);
}
m_renderFrame = 1;
}
}
else if (Mathf.Abs(m_totalRot[0]) > 20.0f || Mathf.Abs(m_totalRot[2]) > 20.0f || (++m_stepCount > (1.0 / simStepSize) * 7.0f))
{
if (DEBUG)
{
Debug.Log("FELL OVER OR GOT STUCK...RESTARTING CURRENT SIM.");
}
if (TRACK_FALLS)
{
m_fallCount++;
// // keep track of fallen ones in a list and move on to next object
if (m_fallCount >= numFallOrStuckLimit)
{
if (DEBUG)
{
Debug.Log("REACHED FALL/STUCK LIMIT, MOVING TO NEXT OBJECT.");
}
m_fallenFiles.Add(m_objFiles[m_curObjIdx]);
if (++m_curObjIdx >= m_objFiles.Length)
{
Debug.Log("BAD FILES: " + m_fallenFiles.Count.ToString() + ", NUM STUCK: " + m_stuckCount.ToString());
for (int k = 0; k < m_fallenFiles.Count; k++)
{
Debug.Log(m_fallenFiles[k]);
}
DumpFallenObjects();
ExitSimulation();
}
else
{
DestroySimObj();
// create new directory to store sims from the next object
m_curDataOutDir = Path.Combine(dataOutDir, m_objFiles[m_curObjIdx].Replace(".obj", "") + "/");
Directory.CreateDirectory(m_curDataOutDir);
PrepareSimObj(m_baseDataDir + m_objFiles[m_curObjIdx], m_masses[m_curObjIdx], m_inertias[m_curObjIdx]);
m_curVideoOutDir = Path.Combine(Application.dataPath, videoOutDir);
m_curVideoOutDir = Path.Combine(m_curVideoOutDir, m_objFiles[m_curObjIdx].Replace(".obj", "") + "/");
Directory.CreateDirectory(m_curVideoOutDir);
string curSimOut = Path.Combine(m_curVideoOutDir, "sim_0");
Directory.CreateDirectory(curSimOut);
m_renderFrame = 1;
// ready the first sim
SetupSim();
SaveInitState();
ClearStateLists();
m_curSimNum = 0;
// reset rotation tracking
m_totalRot = new BulletSharp.Math.Vector3(0.0f, 0.0f, 0.0f);
m_numPerturbs = 0;
}
m_fallCount = 0;
}
}
// the object has probably fallen over, restart
SetupSim();
SaveInitState();
ClearStateLists();
// reset rotation tracking
m_totalRot = new BulletSharp.Math.Vector3(0.0f, 0.0f, 0.0f);
m_numPerturbs = 0;
// } else if (++m_stepCount > (1.0 / simStepSize) * 20) { // been simulating for more than 10 "seconds" (jittering)
// m_stuckCount++;
// if (DEBUG) Debug.Log("SIMULATION STUCK, MOVING TO NEXT OBJECT");
// m_fallenFiles.Add(m_objFiles[m_curObjIdx]);
// if (++m_curObjIdx >= m_objFiles.Length) {
// Debug.Log("BAD FILES: " + m_fallenFiles.Count.ToString() + ", NUM STUCK: " + m_stuckCount.ToString());
// for (int k = 0; k < m_fallenFiles.Count; k++) {
// Debug.Log(m_fallenFiles[k]);
// }
// DumpFallenObjects();
// ExitSimulation();
// } else {
// DestroySimObj();
// // create new directory to store sims from the next object
// m_curDataOutDir = dataOutDir + m_objFiles[m_curObjIdx].Replace(".obj", "") + "/";
// Directory.CreateDirectory(m_curDataOutDir);
// PrepareSimObj(m_baseDataDir + m_objFiles[m_curObjIdx], m_masses[m_curObjIdx], m_inertias[m_curObjIdx]);
// // ready the first sim
// SetupSim();
// SaveInitState();
// m_curSimNum = 0;
// // reset rotation tracking
// m_totalRot = new BulletSharp.Math.Vector3(0.0f, 0.0f, 0.0f);
// }
// m_fallCount = 0;
// // the object has probably fallen over, restart
// SetupSim();
// SaveInitState();
// // reset rotation tracking
// m_totalRot = new BulletSharp.Math.Vector3(0.0f, 0.0f, 0.0f);
}
if (m_stepCount % saveFrequency == 0)
{
SaveCurrentState();
}
if (perturb && m_stepCount != 0 && m_stepCount % perturbFrequency == 0 && m_numPerturbs < maxPerturbations)
{
if (DEBUG)
{
Debug.Log("PERTURBED!");
}
// randomly perturb velocities
Vector2 linPerturb = linearStrength * Random.insideUnitCircle;
m_rb.LinearVelocity += new BulletSharp.Math.Vector3(linPerturb[0], 0.0f, linPerturb[1]);
float angPerturb = angularStrength * Random.Range(0.0f, 1.0f);
m_rb.AngularVelocity += new BulletSharp.Math.Vector3(0.0f, angPerturb, 0.0f);
m_numPerturbs++;
}
//if (DEBUG) Debug.Log("WORLD MOMENT: " + m_rb.InvInertiaTensorWorld.ToString());
//if (DEBUG) Debug.Log("WORLD BASIS: " + m_rb.WorldTransform.Basis.ToString());
}
