public void AddSoftBody(SoftBody body, int collisionFilterGroup, int collisionFilterMask)
{
body.SoftBodySolver = _softBodySolver;
CollisionObjectArray.Add(body, collisionFilterGroup, collisionFilterMask);
}
public void RemoveSoftBody(SoftBody body)
{
CollisionObjectArray.Remove(body);
}
internal void RaiseAddedSoftBody(SoftBody body)
{
if (AddedSoftBody != null) AddedSoftBody(body);
}
public override void Build()
{
AddGround();
for (int i = 0; i < 15; i++)
{
bool even = (i % 2 == 0);
for (int e = 0; e < 3; e++)
{
JVector size = (even) ? new JVector(1, 1, 3) : new JVector(3, 1, 1);
RigidBody body = new RigidBody(new BoxShape(size));
body.Position = new JVector(3.0f + (even ? e : 1.0f), i + 0.5f, -5.0f + (even ? 1.0f : e));
Demo.World.AddBody(body);
}
}
Model model = this.Demo.Content.Load <Model>("torus");
List <TriangleVertexIndices> indices = new List <TriangleVertexIndices>();
List <JVector> vertices = new List <JVector>();
ConvexHullObject.ExtractData(vertices, indices, model);
RemoveDuplicateVertices(indices, vertices);
SoftBody softBody = new SoftBody(indices, vertices);
softBody.Translate(new JVector(10, 5, 0));
softBody.Pressure = 1000.0f;
softBody.SetSpringValues(0.2f, 0.005f);
//softBody.SelfCollision = true; ;
Demo.World.AddBody(softBody);
SoftBody cloth = new SoftBody(20, 20, 0.4f);
// ##### Uncomment for selfcollision, all 3 lines
//cloth.SelfCollision = true;
//cloth.TriangleExpansion = 0.05f;
//cloth.VertexExpansion = 0.05f;
cloth.Translate(new JVector(0, 10, 10));
cloth.Material.KineticFriction = 0.9f;
cloth.Material.StaticFriction = 0.95f;
cloth.VertexBodies[0].IsStatic = true;
cloth.VertexBodies[380].IsStatic = true;
cloth.VertexBodies[19].IsStatic = true;
cloth.VertexBodies[399].IsStatic = true;
cloth.SetSpringValues(SoftBody.SpringType.EdgeSpring, 0.1f, 0.01f);
cloth.SetSpringValues(SoftBody.SpringType.ShearSpring, 0.1f, 0.03f);
cloth.SetSpringValues(SoftBody.SpringType.BendSpring, 0.1f, 0.03f);
// ###### Uncomment here for a better visualization
// Demo.Components.Add(new ClothObject(Demo, cloth));
Demo.World.AddBody(cloth);
}
public void UpdateSoftBody(SoftBody softBody, ShapeData shapeData)
{
AlignedFaceArray faces = softBody.Faces;
if (faces.Count != 0)
{
shapeData.VertexCount = faces.Count * 3;
Vector3[] vectors = new Vector3[shapeData.VertexCount * 2];
int v = 0;
int i;
for (i = 0; i < faces.Count; i++)
{
NodePtrArray nodes = faces[i].N;
Node n0 = nodes[0];
Node n1 = nodes[1];
Node n2 = nodes[2];
n0.GetX(out vectors[v]);
n0.GetNormal(out vectors[v + 1]);
n1.GetX(out vectors[v + 2]);
n1.GetNormal(out vectors[v + 3]);
n2.GetX(out vectors[v + 4]);
n2.GetNormal(out vectors[v + 5]);
v += 6;
}
shapeData.SetDynamicVertexBuffer(device, vectors);
}
else
{
AlignedTetraArray tetras = softBody.Tetras;
int tetraCount = tetras.Count;
if (tetraCount != 0)
{
shapeData.VertexCount = tetraCount * 12;
Vector3[] vectors = new Vector3[tetraCount * 24];
int v = 0;
for (int i = 0; i < tetraCount; i++)
{
NodePtrArray nodes = tetras[i].Nodes;
Vector3 v0 = nodes[0].X;
Vector3 v1 = nodes[1].X;
Vector3 v2 = nodes[2].X;
Vector3 v3 = nodes[3].X;
Vector3 v10 = v1 - v0;
Vector3 v02 = v0 - v2;
Vector3 normal = Vector3.Cross(v10, v02);
vectors[v] = v0;
vectors[v + 1] = normal;
vectors[v + 2] = v1;
vectors[v + 3] = normal;
vectors[v + 4] = v2;
vectors[v + 5] = normal;
normal = Vector3.Cross(v10, v3 - v0);
vectors[v + 6] = v0;
vectors[v + 7] = normal;
vectors[v + 8] = v1;
vectors[v + 9] = normal;
vectors[v + 10] = v3;
vectors[v + 11] = normal;
normal = Vector3.Cross(v2 - v1, v3 - v1);
vectors[v + 12] = v1;
vectors[v + 13] = normal;
vectors[v + 14] = v2;
vectors[v + 15] = normal;
vectors[v + 16] = v3;
vectors[v + 17] = normal;
normal = Vector3.Cross(v02, v3 - v2);
vectors[v + 18] = v2;
vectors[v + 19] = normal;
vectors[v + 20] = v0;
vectors[v + 21] = normal;
vectors[v + 22] = v3;
vectors[v + 23] = normal;
v += 24;
}
shapeData.SetDynamicVertexBuffer(device, vectors);
}
else if (softBody.Links.Count != 0)
{
AlignedLinkArray links = softBody.Links;
int linkCount = links.Count;
shapeData.VertexCount = linkCount * 2;
Vector3[] vectors = new Vector3[linkCount * 4];
for (int i = 0; i < linkCount; i++)
{
NodePtrArray nodes = links[i].Nodes;
nodes[0].GetX(out vectors[i * 4]);
nodes[1].GetX(out vectors[i * 4 + 2]);
}
shapeData.PrimitiveTopology = PrimitiveTopology.LineList;
shapeData.SetDynamicVertexBuffer(device, vectors);
}
else
{
throw new NotImplementedException();
}
}
}
public void AddBody(SoftBody body)
{
if (body == null) throw new ArgumentNullException("body", "body can't be null.");
if (softbodies.Contains(body))
throw new ArgumentException("The body was already added to the world.", "body");
softbodies.Add(body);
CollisionSystem.AddEntity(body);
events.RaiseAddedSoftBody(body);
foreach (Constraint constraint in body.EdgeSprings)
AddConstraint(constraint);
foreach (SoftBody.MassPoint massPoint in body.VertexBodies)
{
events.RaiseAddedRigidBody(massPoint);
rigidBodies.Add(massPoint);
}
}
// Unregister with the physics world
public virtual void Unregister(SoftBody softbody)
{
throw new UnsupportedWorldTypeException(UNSUPPORTED_WORLD_TYPE_MESSAGE);
}
internal void RaiseRemovedSoftBody(SoftBody body)
{
if (RemovedSoftBody != null) RemovedSoftBody(body);
}
public void Unregister(SoftBody body)
{
this.softbodies.Remove(body);
this.World.RemoveCollisionObject(body);
}
private void DetectSoftRigid(RigidBody rigidBody, SoftBody softBody)
{
if (rigidBody.Shape is Multishape)
{
Multishape ms = (rigidBody.Shape as Multishape);
ms = ms.RequestWorkingClone();
JBBox transformedBoundingBox = softBody.BoundingBox;
transformedBoundingBox.InverseTransform(ref rigidBody.position, ref rigidBody.orientation);
int msLength = ms.Prepare(ref transformedBoundingBox);
List <int> detected = potentialTriangleLists.GetNew();
softBody.dynamicTree.Query(detected, ref rigidBody.boundingBox);
foreach (int i in detected)
{
SoftBody.Triangle t = softBody.dynamicTree.GetUserData(i);
JVector point, normal;
float penetration;
bool result;
for (int e = 0; e < msLength; e++)
{
ms.SetCurrentShape(e);
result = XenoCollide.Detect(ms, t, ref rigidBody.orientation, ref JMatrix.InternalIdentity,
ref rigidBody.position, ref JVector.InternalZero, out point, out normal, out penetration);
if (result)
{
int minIndex = FindNearestTrianglePoint(softBody, i, ref point);
RaiseCollisionDetected(rigidBody,
softBody.VertexBodies[minIndex], ref point, ref point, ref normal, penetration);
}
}
}
detected.Clear(); potentialTriangleLists.GiveBack(detected);
ms.ReturnWorkingClone();
}
else
{
List <int> detected = potentialTriangleLists.GetNew();
softBody.dynamicTree.Query(detected, ref rigidBody.boundingBox);
foreach (int i in detected)
{
SoftBody.Triangle t = softBody.dynamicTree.GetUserData(i);
JVector point, normal;
float penetration;
bool result;
result = XenoCollide.Detect(rigidBody.Shape, t, ref rigidBody.orientation, ref JMatrix.InternalIdentity,
ref rigidBody.position, ref JVector.InternalZero, out point, out normal, out penetration);
if (result)
{
int minIndex = FindNearestTrianglePoint(softBody, i, ref point);
RaiseCollisionDetected(rigidBody,
softBody.VertexBodies[minIndex], ref point, ref point, ref normal, penetration);
}
}
detected.Clear();
potentialTriangleLists.GiveBack(detected);
}
}
public void Register(SoftBody body)
{
this.World.AddSoftBody(body);
this.softbodies.Add(body);
}
internal void ProcessDelete(double dt)
{
List <RigidBody> todelete = new List <RigidBody>();
List <int> deleteid = new List <int>();
int cnt = this.RigidBodies.Count;
for (int i = 0; i < cnt; i++)
{
RigidBody body = this.RigidBodies[i];
BodyCustomData bd = (BodyCustomData)body.UserObject;
if (!bd.Created)
{
bd.LifeTime += dt;
}
bd.Created = false;
if (bd.MarkedForDeletion)
{
todelete.Add(body);
deleteid.Add(bd.Id);
}
}
for (int i = 0; i < todelete.Count; i++)
{
RigidBody body = todelete[i];
if (this.RigidBodyDeleted != null)
{
this.RigidBodyDeleted(body, deleteid[i]);
}
this.Unregister(body);
}
cnt = this.SoftBodies.Count;
for (int i = 0; i < cnt; i++)
{
SoftBody body = this.SoftBodies[i];
BodyCustomData bd = (BodyCustomData)body.UserObject;
if (!bd.Created)
{
bd.LifeTime += dt;
}
bd.Created = false;
if (bd.MarkedForDeletion)
{
if (this.SoftBodyDeleted != null)
{
this.SoftBodyDeleted(body, bd.Id);
}
this.Unregister(body);
}
}
cnt = this.Constraints.Count;
for (int i = 0; i < cnt; i++)
{
TypedConstraint cst = this.constraints[i];
ConstraintCustomData cd = (ConstraintCustomData)cst.UserObject;
if (!cd.Created)
{
cd.LifeTime += dt;
}
cd.Created = false;
if (cd.MarkedForDeletion)
{
if (this.ConstraintDeleted != null)
{
this.ConstraintDeleted(cst, cd.Id);
}
this.Unregister(cst);
}
else
{
cd.LifeTime += dt;
}
}
}
public void AddSoftBody(SoftBody body, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask)
{
body.SoftBodySolver = _softBodySolver;
_collisionObjectArray.Add(body, (short)collisionFilterGroup, (short)collisionFilterMask);
}
void Init_ClusterCar()
{
//SetAzi(180);
Vector3 origin = new Vector3(100, 80, 0);
Quaternion orientation = Quaternion.RotationYawPitchRoll(-(float)Math.PI / 2, 0, 0);
const float widthf = 8;
const float widthr = 9;
const float length = 8;
const float height = 4;
Vector3[] wheels = new Vector3[] {
new Vector3(+widthf, -height, +length), // Front left
new Vector3(-widthf, -height, +length), // Front right
new Vector3(+widthr, -height, -length), // Rear left
new Vector3(-widthr, -height, -length), // Rear right
};
SoftBody pa = Create_ClusterBunny(Vector3.Zero, Vector3.Zero);
SoftBody pfl = Create_ClusterTorus(wheels[0], new Vector3(0, 0, (float)Math.PI / 2), new Vector3(2, 4, 2));
SoftBody pfr = Create_ClusterTorus(wheels[1], new Vector3(0, 0, (float)Math.PI / 2), new Vector3(2, 4, 2));
SoftBody prl = Create_ClusterTorus(wheels[2], new Vector3(0, 0, (float)Math.PI / 2), new Vector3(2, 5, 2));
SoftBody prr = Create_ClusterTorus(wheels[3], new Vector3(0, 0, (float)Math.PI / 2), new Vector3(2, 5, 2));
pfl.Cfg.DF =
pfr.Cfg.DF =
prl.Cfg.DF =
prr.Cfg.DF = 1;
LJoint.Specs lspecs = new LJoint.Specs();
lspecs.Cfm = 1;
lspecs.Erp = 1;
lspecs.Position = Vector3.Zero;
lspecs.Position = wheels[0]; pa.AppendLinearJoint(lspecs, pfl);
lspecs.Position = wheels[1]; pa.AppendLinearJoint(lspecs, pfr);
lspecs.Position = wheels[2]; pa.AppendLinearJoint(lspecs, prl);
lspecs.Position = wheels[3]; pa.AppendLinearJoint(lspecs, prr);
AJoint.Specs aspecs = new AJoint.Specs();
aspecs.Cfm = 1;
aspecs.Erp = 1;
aspecs.Axis = new Vector3(1, 0, 0);
aspecs.IControl = steerControlF;
pa.AppendAngularJoint(aspecs, pfl);
pa.AppendAngularJoint(aspecs, pfr);
aspecs.IControl = motorControl;
pa.AppendAngularJoint(aspecs, prl);
pa.AppendAngularJoint(aspecs, prr);
pa.Rotate(orientation);
pfl.Rotate(orientation);
pfr.Rotate(orientation);
prl.Rotate(orientation);
prr.Rotate(orientation);
pa.Translate(origin);
pfl.Translate(origin);
pfr.Translate(origin);
prl.Translate(origin);
prr.Translate(origin);
pfl.Cfg.PIterations =
pfr.Cfg.PIterations =
prl.Cfg.PIterations =
prr.Cfg.PIterations = 1;
pfl.Clusters[0].Matching =
pfr.Clusters[0].Matching =
prl.Clusters[0].Matching =
prr.Clusters[0].Matching = 0.05f;
pfl.Clusters[0].NodeDamping =
pfr.Clusters[0].NodeDamping =
prl.Clusters[0].NodeDamping =
prr.Clusters[0].NodeDamping = 0.05f;
Create_LinearStair(20, new Vector3(0, -8, 0), new Vector3(3, 2, 40));
Create_RbUpStack(50);
//autocam=true;
}
protected override void OnHandleInput()
{
base.OnHandleInput();
if (Input.MousePressed == MouseButtonFlags.RightDown)
{
results.Fraction = 1;
if (pickConstraint == null)
{
Vector3 rayFrom = Freelook.Eye;
Vector3 rayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, FieldOfView);
Vector3 rayDir = (rayTo - rayFrom);
rayDir.Normalize();
AlignedSoftBodyArray sbs = (Physics.World as SoftRigidDynamicsWorld).SoftBodyArray;
for (int ib = 0; ib < sbs.Count; ++ib)
{
SoftBody psb = sbs[ib];
SRayCast res = new SRayCast();
if (psb.RayTest(rayFrom, rayTo, res))
{
results = res;
}
}
if (results.Fraction < 1)
{
impact = rayFrom + (rayTo - rayFrom) * results.Fraction;
drag = !Physics.cutting;
lastMousePos = Input.MousePoint;
node = null;
switch (results.Feature)
{
case EFeature.Tetra:
{
Tetra tet = results.Body.Tetras[results.Index];
node = tet.Nodes[0];
for (int i = 1; i < 4; ++i)
{
if ((node.X - impact).LengthSquared() >
(tet.Nodes[i].X - impact).LengthSquared())
{
node = tet.Nodes[i];
}
}
break;
}
case EFeature.Face:
{
Face f = results.Body.Faces[results.Index];
node = f.N[0];
for (int i = 1; i < 3; ++i)
{
if ((node.X - impact).LengthSquared() >
(f.N[i].X - impact).LengthSquared())
{
node = f.N[i];
}
}
}
break;
}
if (node != null)
{
goal = node.X;
}
//return;
}
}
}
else if (Input.MousePressed == MouseButtonFlags.RightUp)
{
if ((!drag) && Physics.cutting && (results.Fraction < 1))
{
ImplicitSphere isphere = new ImplicitSphere(impact, 1);
results.Body.Refine(isphere, 0.0001f, true);
}
results.Fraction = 1;
drag = false;
}
// Mouse movement
if (Input.MouseDown == MouseButtonFlags.RightDown)
{
if (node != null && (results.Fraction < 1))
{
if (!drag)
{
int x = Input.MousePoint.X - lastMousePos.X;
int y = Input.MousePoint.Y - lastMousePos.Y;
if ((x * x) + (y * y) > 6)
{
drag = true;
}
}
if (drag)
{
lastMousePos = Input.MousePoint;
}
}
}
Physics.HandleInput(Input, FrameDelta);
}
internal void RaiseAddedSoftBody(SoftBody body)
{
if (AddedSoftBody != null) AddedSoftBody(body);
}
private void OnSceneGUI()
{
if (!EditorApplication.isPlaying)
{
Handles.BeginGUI();
GUILayout.Label("Viewing nodes only supported while playing.");
Handles.EndGUI();
return;
}
SoftBody softBody = bSoftBodyTarget.GetCollisionObject() as SoftBody;
if (softBody == null)
{
Handles.BeginGUI();
GUILayout.Label("Seems like the CollisionObject has not been generated.");
Handles.EndGUI();
return;
}
bool clickedNode = false;
AlignedNodeArray nodes = softBody.Nodes;
float highestMass = 0;
for (int i = 0; i < nodes.Count; i++)
{
float invMass = nodes[i].InverseMass;
if (invMass > 0 && 1 / invMass > highestMass)
{
highestMass = 1 / invMass;
}
}
for (int i = 0; i < nodes.Count; i++)
{
float mass = softBody.GetMass(i);
if (_currentSelectedNode == i)
{
Handles.BeginGUI();
EditorGUI.BeginChangeCheck();
EditorGUILayout.LabelField(string.Format("id: {0}", i));
mass = EditorGUILayout.Slider("Mass", mass, 0, Mathf.Clamp(mass * 10, 1, 100), GUILayout.Width(500));
if (EditorGUI.EndChangeCheck())
{
softBody.SetMass(i, mass);
nodes[i].Velocity = BulletSharp.Math.Vector3.Zero;
nodes[i].Force = BulletSharp.Math.Vector3.Zero;
}
Handles.EndGUI();
Handles.color = Color.green;
}
else
{
Color usedColor;
if (mass > 0)
{
usedColor = Color.Lerp(Color.white, Color.gray, mass / highestMass);
}
else
{
usedColor = Color.blue;
}
Handles.color = usedColor;
}
Vector3 position = nodes[i].Position.ToUnity();
float size = HandleUtility.GetHandleSize(position) * 0.5f;
if (Handles.Button(position, Quaternion.identity, size, size, Handles.SphereHandleCap))
{
clickedNode = true;
_currentSelectedNode = i;
}
}
if (!clickedNode && Input.GetMouseButton(0))
{
_currentSelectedNode = -1;
}
}
public void Register(SoftBody body)
{
this.dynamicsWorld.AddSoftBody(body);
this.softBodyContainer.RegisterObject(body);
}
internal void RaiseAddedSoftBody(SoftBody body)
{
AddedSoftBody?.Invoke(body);
}
private void DrawDynamicTree(SoftBody cloth)
{
Walk(cloth.DynamicTree, cloth.DynamicTree.Root);
}
internal void RaiseRemovedSoftBody(SoftBody body)
{
RemovedSoftBody?.Invoke(body);
}
public bool RemoveBody(SoftBody body)
{
if (!softbodies.Remove(body)) return false;
CollisionSystem.RemoveEntity(body);
events.RaiseRemovedSoftBody(body);
foreach (Constraint constraint in body.EdgeSprings)
RemoveConstraint(constraint);
foreach (SoftBody.MassPoint massPoint in body.VertexBodies)
RemoveBody(massPoint, true);
return true;
}
public void AddSoftBody(SoftBody body)
{
body.SoftBodySolver = _softBodySolver;
CollisionObjectArray.Add(body);
}
internal void RaiseRemovedSoftBody(SoftBody body)
{
if (RemovedSoftBody != null) RemovedSoftBody(body);
}
SoftBody CreateVolumetricSoftbody(SoftBodyWorldInfo worldInfo, BulletSharp.Math.Vector3[] vertices)
{
int vertexcount = vertices.Length;
psb = new SoftBody(worldInfo, vertexcount, vertices, null);
for (int x = 3; vertexcount > x; x += 4)
{
int n0 = x - 3;
int n1 = x - 2;
int n2 = x - 1;
int n3 = x - 0;
psb.AppendTetra(n0, n1, n2, n3);
psb.AppendLink(n0, n1);
linkdata.Add(true);
brokenlinks.Add(false);
psb.AppendLink(n1, n2);
linkdata.Add(true);
brokenlinks.Add(false);
psb.AppendLink(n2, n0);
linkdata.Add(true);
brokenlinks.Add(false);
psb.AppendLink(n0, n3);
linkdata.Add(true);
brokenlinks.Add(false);
psb.AppendLink(n1, n3);
linkdata.Add(true);
brokenlinks.Add(false);
psb.AppendLink(n2, n3);
linkdata.Add(true);
brokenlinks.Add(false);
ValidTetras.Add(true);
}
instances = GetInstances(vertices);
//List<bool> ValidCuts = CutTest3(vertices);
for (int x = 0; instances.Count > x; x++)
{
//if (ValidCuts[x] == false)
{
// continue;
}
for (int i = 1; instances[x].Count > i; i++)
{
int i1 = instances[x][i - 1];
int i2 = instances[x][i - 0];
psb.AppendLink(i1, i2);
linkdata.Add(false);
brokenlinks.Add(false);
}
}
return(psb);
}
