protected void SaveCurrentState() { m_stepVel.Add(BSExtensionMethods2.ToUnity(m_rb.LinearVelocity)); m_stepAngVel.Add(BSExtensionMethods2.ToUnity(m_rb.AngularVelocity)); Matrix curTrans = m_rb.WorldTransform; m_stepRot.Add(BSExtensionMethods2.ExtractRotationFromMatrix(ref curTrans)); m_stepEulerRot.Add(BSExtensionMethods2.ToUnity(m_totalRot)); m_stepPos.Add(BSExtensionMethods2.ToUnity(m_rb.CenterOfMassPosition)); m_stepNum.Add(m_stepCount); }
protected void DumpSimInfo() { // write out to json if (DEBUG) { Debug.Log("Finished Trial " + (m_curSimNum + 1) + ", Saving Data..."); } // always print out status every 250 sims if (m_curSimNum % 50 == 0) { Debug.Log("Finished Sim " + m_curSimNum + " for object " + m_curObjIdx + "..."); } // output to data file SimSaveData data = new SimSaveData(); data.shape = dataInDir + "/" + m_objFiles[m_curObjIdx].Replace(".obj", ""); data.scale = m_pclScale; data.mass = m_mass; data.density = m_density; data.inertia = BSExtensionMethods2.ToUnity(m_inertia); data.forcePoint = BSExtensionMethods2.ToUnity(m_forcePoint); data.forceVec = BSExtensionMethods2.ToUnity(m_forceVec); data.vel0 = BSExtensionMethods2.ToUnity(m_vel0); data.angvel0 = BSExtensionMethods2.ToUnity(m_angvel0); data.com0 = BSExtensionMethods2.ToUnity(m_com0); data.pos0 = m_pos0; data.rot0 = m_rot0; data.eulerrot0 = m_eulerrot0; data.comf = BSExtensionMethods2.ToUnity(m_comf); data.posf = m_posf; data.rotf = m_rotf; data.eulerrotf = m_eulerrotf; data.totalRot = BSExtensionMethods2.ToUnity(m_totalRot); data.stepVel = m_stepVel; data.stepAngVel = m_stepAngVel; data.stepRot = m_stepRot; data.stepEulerRot = m_stepEulerRot; data.stepPos = m_stepPos; data.stepNum = m_stepNum; string dataStr = data.SaveString(); if (DEBUG) { Debug.Log(dataStr); } string outFile = m_curDataOutDir + "sim_" + m_curSimNum.ToString() + ".json"; StreamWriter writer = new StreamWriter(outFile, false); writer.WriteLine(dataStr); writer.Close(); }
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()); }