//public NativeArray<Matrix4x4> m;
public void Execute(int i, TransformAccess transform)
{
Matrix4x4 m = PhysicsDemo_BulletJobs.CollisionObjects[i].WorldTransform.ToUnity();
transform.position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
transform.rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref m);
}
// Update is called once per frame
void Update()
{
Matrix4x4 m = target.WorldTransform.ToUnity();
transform.position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
transform.rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref m);
}
public void FixedUpdate()
{
BulletSharp.Math.Matrix trans;
ghostObject.GetWorldTransform(out trans);
transform.position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref trans);
transform.rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref trans);
transform.localScale = BSExtensionMethods2.ExtractScaleFromMatrix(ref trans);
}
protected void SaveFinalState()
{
m_comf = m_rb.CenterOfMassPosition;
Matrix curTrans = m_rb.WorldTransform;
m_posf = BSExtensionMethods2.ExtractTranslationFromMatrix(ref curTrans);
m_rotf = BSExtensionMethods2.ExtractRotationFromMatrix(ref curTrans);
m_eulerrotf = m_rotf.eulerAngles;
}
public void Execute(int i)
{
Matrix4x4 m = PhysicsDemo_BulletECS.CollisionObjects[i].WorldTransform.ToUnity();
positions[i] = new Position {
Value = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m)
};
rotations[i] = new Rotation {
Value = BSExtensionMethods2.ExtractRotationFromMatrix(ref m)
};
}
// Update is called once per frame
void Update()
{
if (target == null)
{
return;
}
Matrix4x4 m = target.WorldTransform.ToUnity();
transform.position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
transform.rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref m);
transform.localScale = BSExtensionMethods2.ExtractScaleFromMatrix(ref m);
}
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();
}
void DoEverything()
{
OnInitializePhysics();
for (int i = 0; i < 100; i++)
{
OnUpdate();
for (int j = 0; j < numLinks; j++)
{
MultiBodyLinkCollider linkCollider = links[j];
UnityEngine.Matrix4x4 m = linkCollider.WorldTransform.ToUnity();
UnityEngine.Vector3 p = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
linkPositions[j] = p;
UnityEngine.Debug.Log("pos " + p.ToString("f3") + " " + j);
}
}
ExitPhysics();
}
// 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;
}
IEnumerator myCoroutine()
{
Debug.Log("Waiting to start");
while (!Input.GetKeyDown(KeyCode.Space))
{
yield return(null);
}
OnInitializePhysics();
Debug.Log("Initialized");
int frame = 0;
while (frame < 10)
{
if (Input.GetKeyDown(KeyCode.Space))
{
frame++;
Debug.Log("Do Update " + frame);
for (int j = 0; j < numLinks; j++)
{
MultiBodyLinkCollider linkCollider = links[j];
for (int k = 0; k < 5; k++)
{
OnUpdate();
}
UnityEngine.Matrix4x4 m = linkCollider.WorldTransform.ToUnity();
UnityEngine.Vector3 p = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
linkPositions[j] = p;
UnityEngine.Debug.Log("pos " + p.ToString("f3") + " " + j);
}
}
yield return(null);
}
Debug.Log("Finalize");
ExitPhysics();
}
public void CreateUnityMultiBodyLinkColliderProxy(MultiBodyLinkCollider body)
{
GameObject cube = Instantiate <GameObject>(cubePrefab);
CollisionShape cs = body.CollisionShape;
if (cs is BoxShape)
{
BoxShape bxcs = cs as BoxShape;
BulletSharp.Math.Vector3 s = bxcs.HalfExtentsWithMargin;
MeshRenderer mr = cube.GetComponentInChildren <MeshRenderer>();
mr.transform.localScale = s.ToUnity() * 2f;
Matrix4x4 m = body.WorldTransform.ToUnity();
cube.transform.position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m);
cube.transform.rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref m);
cube.transform.localScale = BSExtensionMethods2.ExtractScaleFromMatrix(ref m);
Destroy(cube.GetComponent <BulletRigidBodyProxy>());
BulletMultiBodyLinkColliderProxy cp = cube.AddComponent <BulletMultiBodyLinkColliderProxy>();
cp.target = body;
}
else
{
Debug.LogError("Not implemented");
}
}
public void PostOnInitializePhysics()
{
for (int i = 0; i < demo.World.CollisionObjectArray.Count; i++)
{
CollisionObject co = demo.World.CollisionObjectArray[i];
CollisionShape cs = co.CollisionShape;
GameObject go;
if (cs.ShapeType == BroadphaseNativeType.SoftBodyShape)
{
BulletSharp.SoftBody.SoftBody sb = (BulletSharp.SoftBody.SoftBody)co;
if (sb.Faces.Count == 0)
{
//rope
go = CreateUnitySoftBodyRope(sb);
}
else
{
go = CreateUnitySoftBodyCloth(sb);
}
}
else
{
//rigid body
if (cs.ShapeType == BroadphaseNativeType.CompoundShape)
{
BulletSharp.Math.Matrix transform = co.WorldTransform;
go = new GameObject("Compund Shape");
BulletRigidBodyProxy rbp = go.AddComponent <BulletRigidBodyProxy>();
rbp.target = co as RigidBody;
foreach (BulletSharp.CompoundShapeChild child in (cs as CompoundShape).ChildList)
{
BulletSharp.Math.Matrix childTransform = child.Transform;
GameObject ggo = new GameObject(child.ToString());
MeshFilter mf = ggo.AddComponent <MeshFilter>();
Mesh m = mf.mesh;
MeshFactory2.CreateShape(child.ChildShape, m);
MeshRenderer mr = ggo.AddComponent <MeshRenderer>();
mr.sharedMaterial = mat;
ggo.transform.SetParent(go.transform);
Matrix4x4 mt = childTransform.ToUnity();
ggo.transform.localPosition = BSExtensionMethods2.ExtractTranslationFromMatrix(ref mt);
ggo.transform.localRotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref mt);
ggo.transform.localScale = BSExtensionMethods2.ExtractScaleFromMatrix(ref mt);
/*
* BulletRigidBodyProxy rbp = ggo.AddComponent<BulletRigidBodyProxy>();
* rbp.target = body;
* return go;
*/
//InitRigidBodyInstance(colObj, child.ChildShape, ref childTransform);
}
}
else if (cs.ShapeType == BroadphaseNativeType.CapsuleShape)
{
GameObject ggo = GameObject.CreatePrimitive(PrimitiveType.Capsule);
Destroy(ggo.GetComponent <Collider>());
go = new GameObject();
ggo.transform.parent = go.transform;
ggo.transform.localPosition = Vector3.zero;
ggo.transform.localRotation = Quaternion.identity;
BulletRigidBodyProxy rbp = go.AddComponent <BulletRigidBodyProxy>();
rbp.target = co as RigidBody;
}
else
{
Debug.Log("Creating " + cs.ShapeType + " for " + co.ToString());
go = CreateUnityCollisionObjectProxy(co as CollisionObject);
}
}
createdObjs.Add(go);
Debug.Log("Created Unity Shape for " + co);
}
}
// Creates a Unity game object from the given Bullet CollisionObject.
protected void AddUnityObject(CollisionObject co, Material mat)
{
CollisionShape cs = co.CollisionShape;
GameObject go;
if (cs.ShapeType == BroadphaseNativeType.SoftBodyShape)
{
BulletSharp.SoftBody.SoftBody sb = (BulletSharp.SoftBody.SoftBody)co;
if (sb.Faces.Count == 0)
{
//rope
go = CreateUnitySoftBodyRope(sb);
}
else
{
go = CreateUnitySoftBodyCloth(sb);
}
}
else
{
//rigid body
if (cs.ShapeType == BroadphaseNativeType.CompoundShape)
{
//BulletSharp.Math.Matrix transform = co.WorldTransform;
go = new GameObject("Compund Shape");
BulletRigidBodyProxy rbp = go.AddComponent <BulletRigidBodyProxy>();
rbp.target = co as RigidBody;
foreach (BulletSharp.CompoundShapeChild child in (cs as CompoundShape).ChildList)
{
BulletSharp.Math.Matrix childTransform = child.Transform;
GameObject ggo = new GameObject(child.ToString());
MeshFilter mf = ggo.AddComponent <MeshFilter>();
Mesh m = mf.mesh;
MeshFactory2.CreateShape(child.ChildShape, m);
MeshRenderer mr = ggo.AddComponent <MeshRenderer>();
mr.sharedMaterial = mat;
ggo.transform.SetParent(go.transform);
Matrix4x4 mt = childTransform.ToUnity();
ggo.transform.localPosition = BSExtensionMethods2.ExtractTranslationFromMatrix(ref mt);
ggo.transform.localRotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref mt);
ggo.transform.localScale = BSExtensionMethods2.ExtractScaleFromMatrix(ref mt);
/*
* BulletRigidBodyProxy rbp = ggo.AddComponent<BulletRigidBodyProxy>();
* rbp.target = body;
* return go;
*/
//InitRigidBodyInstance(colObj, child.ChildShape, ref childTransform);
}
}
else if (cs.ShapeType == BroadphaseNativeType.CapsuleShape)
{
CapsuleShape css = (CapsuleShape)cs;
GameObject ggo = GameObject.CreatePrimitive(PrimitiveType.Capsule);
Destroy(ggo.GetComponent <Collider>());
go = new GameObject();
ggo.transform.parent = go.transform;
ggo.transform.localPosition = UnityEngine.Vector3.zero;
ggo.transform.localRotation = UnityEngine.Quaternion.identity;
ggo.transform.localScale = new UnityEngine.Vector3(css.Radius * 2f, css.HalfHeight * 2f, css.Radius * 2f);
BulletRigidBodyProxy rbp = go.AddComponent <BulletRigidBodyProxy>();
rbp.target = co;
}
else
{
//Debug.Log("Creating " + cs.ShapeType + " for " + co.ToString());
go = CreateUnityCollisionObjectProxy(co as CollisionObject, mat);
}
}
m_createdObjs.Add(go);
}
private void Update()
{
target.WorldTransform.ToUnity(ref m);
_transform.position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m, ref tempVector3);
_transform.rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref m, ref tempQuaternion);
}
void SetGameObjPosFromCollisionObj()
{
collisionObject.GetWorldTransform(out Matrix trans);
transform.position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref trans);
}
// 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());
}
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());
}