// Update is called once per frame
public void Update()
{
lock (lck)
{
if (mustUpdateTransform)
{
UnityEngine.Vector3 position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref lastBulletTransform.Transform);
UnityEngine.Quaternion rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref lastBulletTransform.Transform);
// Interpolation is needed in threaded mode
// Maybe we can have a call to the physics engine in an update method instead ?
if (threadHelper != null && previousBulletTransform != null)
{
double currentTime = threadHelper.TotalSimulationTime;
UnityEngine.Vector3 previousPosition = BSExtensionMethods2.ExtractTranslationFromMatrix(ref previousBulletTransform.Transform);
UnityEngine.Quaternion previousRotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref previousBulletTransform.Transform);
double interpolationFactor = (currentTime - lastBulletTransform.TimeStamp) / (lastBulletTransform.TimeStamp - previousBulletTransform.TimeStamp);
transform.position = UnityEngine.Vector3.LerpUnclamped(previousPosition, position, (float)interpolationFactor);
transform.rotation = UnityEngine.Quaternion.SlerpUnclamped(previousRotation, rotation, (float)interpolationFactor);
}
else
{
transform.position = position;
transform.rotation = rotation;
}
mustUpdateTransform = false;
}
pos = transform.position.ToBullet();
rot = transform.rotation.ToBullet();
}
}
public BGameObjectMotionState(Transform transform)
{
this.transform = transform;
pos = transform.position.ToBullet();
rot = transform.rotation.ToBullet();
threadHelper = BPhysicsWorld.Get().PhysicsWorldHelper as BThreadedWorldHelper;
}
//Bullet wants me to fill in worldTrans
//This is called by bullet once when rigid body is added to the the world
//For kinematic rigid bodies it is called every simulation step
//[MonoPInvokeCallback(typeof(GetTransformDelegate))]
public override void GetWorldTransform(out BM.Matrix worldTrans)
{
//Matrix4x4 trans = transform.localToWorldMatrix;
//worldTrans = trans.ToBullet();
BulletSharp.Math.Quaternion q = transform.rotation.ToBullet();
BulletSharp.Math.Matrix.RotationQuaternion(ref q, out worldTrans);
worldTrans.Origin = transform.position.ToBullet();
}
/// <summary>
/// Slightly modified BPairCachingGhostObject's _BuildCollisionObject method, sets local variables.
/// Done on Initialization.
/// </summary>
internal override bool _BuildCollisionObject()
{
#region Remove old collision object.
BPhysicsWorld world = BPhysicsWorld.Get();
if (m_collisionObject != null)
{
if (isInWorld && world != null)
{
isInWorld = false;
world.RemoveCollisionObject(this);
}
}
#endregion
#region Warnings and Errors
//Friendly note
if (collisionShape.transform.localScale != UnityEngine.Vector3.one)
{
Debug.LogWarning("The local scale on this collision shape is not one. Bullet physics does not support scaling on a rigid body world transform. Instead alter the dimensions of the CollisionShape. " + name, this);
}
if (collisionShape == null)
{
Debug.LogError("There was no collision shape set. " + name, this);
return(false);
}
if (!(collisionShape.GetCollisionShape() is ConvexShape))
{
Debug.LogError("The CollisionShape on this character controller is not a convex shape. " + name, this);
return(false);
}
#endregion
#region Init PairCachingGhostObject and ConvexShape
m_collisionObject = new PairCachingGhostObject();
m_collisionShape = collisionShape;
m_collisionObject.CollisionShape = m_collisionShape.GetCollisionShape();
m_collisionObject.CollisionFlags = m_collisionFlags;
BulletSharp.Math.Matrix worldTrans;
BulletSharp.Math.Quaternion q = transform.rotation.ToBullet();
BulletSharp.Math.Matrix.RotationQuaternion(ref q, out worldTrans);
worldTrans.Origin = transform.position.ToBullet();
m_collisionObject.WorldTransform = worldTrans;
m_collisionObject.UserObject = this;
#endregion
ghostObject = (PairCachingGhostObject)m_collisionObject;
convexShape = (ConvexShape)m_collisionShape.GetCollisionShape();
world.AddAction(this);
return(true);
}
public static Vector3 ToEulerAngles(this Quaternion q)
{
var sqw = q.W * q.W;
var sqx = q.X * q.X;
var sqy = q.Y * q.Y;
var sqz = q.Z * q.Z;
return(new Vector3(
(float)Math.Atan2(2f * (q.Y * q.Z + q.X * q.W), -sqx - sqy + sqz + sqw),
(float)-Math.Asin(2f * (q.X * q.Z - q.Y * q.W)),
(float)Math.Atan2(2f * (q.X * q.Y + q.Z * q.W), sqx - sqy - sqz + sqw)));
}
public static void SetOrientation(this Matrix bm, Quaternion q)
{
var xx = q.X * q.X;
var yy = q.Y * q.Y;
var zz = q.Z * q.Z;
var xy = q.X * q.Y;
var zw = q.Z * q.W;
var zx = q.Z * q.X;
var yw = q.Y * q.W;
var yz = q.Y * q.Z;
var xw = q.X * q.W;
bm.M11 = 1.0f - (2.0f * (yy + zz));
bm.M12 = 2.0f * (xy + zw);
bm.M13 = 2.0f * (zx - yw);
bm.M21 = 2.0f * (xy - zw);
bm.M22 = 1.0f - (2.0f * (zz + xx));
bm.M23 = 2.0f * (yz + xw);
bm.M31 = 2.0f * (zx + yw);
bm.M32 = 2.0f * (yz - xw);
bm.M33 = 1.0f - (2.0f * (yy + xx));
}
//Bullet wants me to fill in worldTrans
//This is called by bullet once when rigid body is added to the the world
//For kinematic rigid bodies it is called every simulation step
//[MonoPInvokeCallback(typeof(GetTransformDelegate))]
public override void GetWorldTransform(out BM.Matrix worldTrans)
{
BulletSharp.Math.Vector3 pos = unit.Position.ToBullet();
BulletSharp.Math.Quaternion rot = unit.Quaternion.ToBullet();
BulletSharp.Math.Matrix.AffineTransformation(1f, ref rot, ref pos, out worldTrans);
}
//IMPORTANT Time.fixedTime must match the timestep being used here.
public static List <UnityEngine.Vector3> SimulateBall(BRigidBody ballRb, UnityEngine.Vector3 ballThrowForce, int numberOfSimulationSteps, bool reverseOrder)
{
var ballPositions = new List <UnityEngine.Vector3>(numberOfSimulationSteps);
//Create a World
Debug.Log("Initialize physics");
CollisionConfiguration CollisionConf;
CollisionDispatcher Dispatcher;
BroadphaseInterface Broadphase;
CollisionWorld cw;
ConstraintSolver Solver;
BulletSharp.SoftBody.SoftBodyWorldInfo softBodyWorldInfo;
//This should create a copy of the BPhysicsWorld with the same settings
BPhysicsWorld bw = BPhysicsWorld.Get();
bw.CreatePhysicsWorld(out cw, out CollisionConf, out Dispatcher, out Broadphase, out Solver, out softBodyWorldInfo);
World = (DiscreteDynamicsWorld)cw;
//Copy all existing rigidbodies in scene
// IMPORTANT rigidbodies must be added to the offline world in the same order that they are in the source world
// this is because collisions must be resolved in the same order for the sim to be deterministic
DiscreteDynamicsWorld sourceWorld = (DiscreteDynamicsWorld)bw.world;
BulletSharp.RigidBody bulletBallRb = null;
BulletSharp.Math.Matrix mm = BulletSharp.Math.Matrix.Identity;
for (int i = 0; i < sourceWorld.NumCollisionObjects; i++)
{
CollisionObject co = sourceWorld.CollisionObjectArray[i];
if (co != null && co.UserObject is BRigidBody)
{
BRigidBody rb = (BRigidBody)co.UserObject;
float mass = rb.isDynamic() ? rb.mass : 0f;
BCollisionShape existingShape = rb.GetComponent <BCollisionShape>();
CollisionShape shape = null;
if (existingShape is BSphereShape)
{
shape = ((BSphereShape)existingShape).CopyCollisionShape();
}
else if (existingShape is BBoxShape)
{
shape = ((BBoxShape)existingShape).CopyCollisionShape();
}
RigidBody bulletRB = null;
BulletSharp.Math.Vector3 localInertia = new BulletSharp.Math.Vector3();
rb.CreateOrConfigureRigidBody(ref bulletRB, ref localInertia, shape, null);
BulletSharp.Math.Vector3 pos = rb.GetCollisionObject().WorldTransform.Origin;
BulletSharp.Math.Quaternion rot = rb.GetCollisionObject().WorldTransform.GetOrientation();
BulletSharp.Math.Matrix.AffineTransformation(1f, ref rot, ref pos, out mm);
bulletRB.WorldTransform = mm;
World.AddRigidBody(bulletRB, rb.groupsIBelongTo, rb.collisionMask);
if (rb == ballRb)
{
bulletBallRb = bulletRB;
bulletRB.ApplyCentralImpulse(ballThrowForce.ToBullet());
}
}
}
//Step the simulation numberOfSimulationSteps times
for (int i = 0; i < numberOfSimulationSteps; i++)
{
int numSteps = World.StepSimulation(1f / 60f, 10, 1f / 60f);
ballPositions.Add(bulletBallRb.WorldTransform.Origin.ToUnity());
}
UnityEngine.Debug.Log("ExitPhysics");
if (World != null)
{
//remove/dispose constraints
int i;
for (i = World.NumConstraints - 1; i >= 0; i--)
{
TypedConstraint constraint = World.GetConstraint(i);
World.RemoveConstraint(constraint);
constraint.Dispose();
}
//remove the rigidbodies from the dynamics world and delete them
for (i = World.NumCollisionObjects - 1; i >= 0; i--)
{
CollisionObject obj = World.CollisionObjectArray[i];
RigidBody body = obj as RigidBody;
if (body != null && body.MotionState != null)
{
body.MotionState.Dispose();
}
World.RemoveCollisionObject(obj);
obj.Dispose();
}
World.Dispose();
Broadphase.Dispose();
Dispatcher.Dispose();
CollisionConf.Dispose();
}
if (Broadphase != null)
{
Broadphase.Dispose();
}
if (Dispatcher != null)
{
Dispatcher.Dispose();
}
if (CollisionConf != null)
{
CollisionConf.Dispose();
}
return(ballPositions);
}
//todo(erwincoumans) Quick hack, reference to InvertedPendulumPDControl implementation. Will create a separate header/source file for this.
public static MultiBody createInvertedPendulumMultiBody(float radius, MultiBodyDynamicsWorld world, Matrix baseWorldTrans, bool fixedBase)
{
BulletSharp.Math.Vector4[] colors = new BulletSharp.Math.Vector4[]
{
new BulletSharp.Math.Vector4(1, 0, 0, 1),
new BulletSharp.Math.Vector4(0, 1, 0, 1),
new BulletSharp.Math.Vector4(0, 1, 1, 1),
new BulletSharp.Math.Vector4(1, 1, 0, 1),
};
int curColor = 0;
bool damping = false;
bool gyro = false;
int numLinks = 2;
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool canSleep = false;
bool selfCollide = false;
BulletSharp.Math.Vector3 linkHalfExtents = new BulletSharp.Math.Vector3(0.05f, 0.37f, 0.1f);
BulletSharp.Math.Vector3 baseHalfExtents = new BulletSharp.Math.Vector3(0.04f, 0.35f, 0.08f);
//mbC.forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
//init the base
BulletSharp.Math.Vector3 baseInertiaDiag = new BulletSharp.Math.Vector3(0.0f, 0.0f, 0.0f);
float baseMass = fixedBase ? 0.0f : 10.0f;
if (baseMass != 0)
{
//CollisionShape *shape = new btSphereShape(baseHalfExtents[0]);// btBoxShape(BulletSharp.Math.Vector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
CollisionShape shape = new BoxShape(new BulletSharp.Math.Vector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
shape.CalculateLocalInertia(baseMass, out baseInertiaDiag);
shape.Dispose();
}
MultiBody pMultiBody = new MultiBody(numLinks, 0, baseInertiaDiag, fixedBase, canSleep);
pMultiBody.BaseWorldTransform = baseWorldTrans;
BulletSharp.Math.Vector3 vel = new BulletSharp.Math.Vector3(0, 0, 0);
// pMultiBody.setBaseVel(vel);
//init the links
BulletSharp.Math.Vector3 hingeJointAxis = new BulletSharp.Math.Vector3(1, 0, 0);
//y-axis assumed up
BulletSharp.Math.Vector3 parentComToCurrentCom = new BulletSharp.Math.Vector3(0, -linkHalfExtents[1] * 2.0f, 0); //par body's COM to cur body's COM offset
BulletSharp.Math.Vector3 currentPivotToCurrentCom = new BulletSharp.Math.Vector3(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset
BulletSharp.Math.Vector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
//////
float q0 = 1.0f * Mathf.PI / 180.0f;
BulletSharp.Math.Quaternion quat0 = new BulletSharp.Math.Quaternion(new BulletSharp.Math.Vector3(1, 0, 0), q0);
quat0.Normalize();
/////
for (int i = 0; i < numLinks; ++i)
{
float linkMass = 1.0f;
//if (i==3 || i==2)
// linkMass= 1000;
BulletSharp.Math.Vector3 linkInertiaDiag = new BulletSharp.Math.Vector3(0.0f, 0.0f, 0.0f);
CollisionShape shape = null;
if (i == 0)
{
shape = new BoxShape(new BulletSharp.Math.Vector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));//
}
else
{
shape = new SphereShape(radius);
}
shape.CalculateLocalInertia(linkMass, out linkInertiaDiag);
shape.Dispose();
if (!spherical)
{
//pMultiBody.setupRevolute(i, linkMass, linkInertiaDiag, i - 1, BulletSharp.Math.Quaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
if (i == 0)
{
pMultiBody.SetupRevolute(i, linkMass, linkInertiaDiag, i - 1,
new BulletSharp.Math.Quaternion(0.0f, 0.0f, 0.0f, 1.0f),
hingeJointAxis,
parentComToCurrentPivot,
currentPivotToCurrentCom, false);
}
else
{
parentComToCurrentCom = new BulletSharp.Math.Vector3(0, -radius * 2.0f, 0); //par body's COM to cur body's COM offset
currentPivotToCurrentCom = new BulletSharp.Math.Vector3(0, -radius, 0); //cur body's COM to cur body's PIV offset
parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
pMultiBody.SetupFixed(i, linkMass, linkInertiaDiag, i - 1,
new BulletSharp.Math.Quaternion(0.0f, 0.0f, 0.0f, 1.0f),
parentComToCurrentPivot,
currentPivotToCurrentCom);
}
}
else
{
//pMultiBody.setupPlanar(i, linkMass, linkInertiaDiag, i - 1, BulletSharp.Math.Quaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, BulletSharp.Math.Vector3(1, 0, 0), parentComToCurrentPivot*2, false);
pMultiBody.SetupSpherical(i, linkMass, linkInertiaDiag, i - 1, new BulletSharp.Math.Quaternion(0.0f, 0.0f, 0.0f, 1.0f), parentComToCurrentPivot, currentPivotToCurrentCom, false);
}
}
pMultiBody.FinalizeMultiDof();
world.AddMultiBody(pMultiBody);
MultiBody mbC = pMultiBody;
mbC.CanSleep = (canSleep);
mbC.HasSelfCollision = (selfCollide);
mbC.UseGyroTerm = (gyro);
//
if (!damping)
{
mbC.LinearDamping = (0.0f);
mbC.AngularDamping = (0.0f);
}
else
{
mbC.LinearDamping = (0.1f);
mbC.AngularDamping = (0.9f);
}
if (numLinks > 0)
{
q0 = 180.0f * Mathf.PI / 180.0f;
if (!spherical)
{
mbC.SetJointPosMultiDof(0, new float[] { q0 });
}
else
{
BulletSharp.Math.Vector3 vv = new BulletSharp.Math.Vector3(1, 1, 0);
vv.Normalize();
quat0 = new BulletSharp.Math.Quaternion(vv, q0);
quat0.Normalize();
float[] quat0fs = new float[] { quat0.X, quat0.Y, quat0.Z, quat0.W };
mbC.SetJointPosMultiDof(0, quat0fs);
}
}
///
BulletSharp.Math.Quaternion[] world_to_local; //btAlignedObjectArray<BulletSharp.Math.Quaternion>
world_to_local = new BulletSharp.Math.Quaternion[pMultiBody.NumLinks + 1];
BulletSharp.Math.Vector3[] local_origin; //btAlignedObjectArray<BulletSharp.Math.Vector3>
local_origin = new BulletSharp.Math.Vector3[pMultiBody.NumLinks + 1];
world_to_local[0] = pMultiBody.WorldToBaseRot;
local_origin[0] = pMultiBody.BasePosition;
// double friction = 1;
{
if (true)
{
CollisionShape shape = new BoxShape(new BulletSharp.Math.Vector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); //new btSphereShape(baseHalfExtents[0]);
// guiHelper.createCollisionShapeGraphicsObject(shape);
MultiBodyLinkCollider col = new MultiBodyLinkCollider(pMultiBody, -1);
col.CollisionShape = shape;
Matrix tr = new Matrix();
tr.ScaleVector = BulletSharp.Math.Vector3.One;
//if we don't set the initial pose of the btCollisionObject, the simulator will do this
//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
tr.Origin = local_origin[0];
BulletSharp.Math.Quaternion orn = new BulletSharp.Math.Quaternion(new BulletSharp.Math.Vector3(0, 0, 1), 0.25f * 3.1415926538f);
tr.Rotation = (orn);
col.WorldTransform = (tr);
bool isDynamic = (baseMass > 0 && !fixedBase);
CollisionFilterGroups collisionFilterGroup = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter;
CollisionFilterGroups collisionFilterMask = isDynamic ? CollisionFilterGroups.AllFilter : CollisionFilterGroups.AllFilter ^ CollisionFilterGroups.StaticFilter;
world.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask);//, 2,1+2);
BulletSharp.Math.Vector4 color = new BulletSharp.Math.Vector4(0.0f, 0.0f, 0.5f, 1f);
//guiHelper.createCollisionObjectGraphicsObject(col, color);
// col.setFriction(friction);
pMultiBody.BaseCollider = (col);
}
}
for (int i = 0; i < pMultiBody.NumLinks; ++i)
{
int parent = pMultiBody.GetParent(i);
world_to_local[i + 1] = pMultiBody.GetParentToLocalRot(i) * world_to_local[parent + 1];
BulletSharp.Math.Vector3 vv = world_to_local[i + 1].Inverse.Rotate(pMultiBody.GetRVector(i));
local_origin[i + 1] = local_origin[parent + 1] + vv;
}
for (int i = 0; i < pMultiBody.NumLinks; ++i)
{
BulletSharp.Math.Vector3 posr = local_origin[i + 1];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
float[] quat = new float[] { -world_to_local[i + 1].X, -world_to_local[i + 1].Y, -world_to_local[i + 1].Z, world_to_local[i + 1].W };
CollisionShape shape = null;
if (i == 0)
{
shape = new BoxShape(new BulletSharp.Math.Vector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]);
}
else
{
shape = new SphereShape(radius);
}
//guiHelper.createCollisionShapeGraphicsObject(shape);
MultiBodyLinkCollider col = new MultiBodyLinkCollider(pMultiBody, i);
col.CollisionShape = (shape);
Matrix tr = new Matrix();
tr.ScaleVector = new BulletSharp.Math.Vector3();
tr.Origin = (posr);
tr.Rotation = (new BulletSharp.Math.Quaternion(quat[0], quat[1], quat[2], quat[3]));
col.WorldTransform = (tr);
// col.setFriction(friction);
bool isDynamic = true;//(linkMass > 0);
CollisionFilterGroups collisionFilterGroup = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter;
CollisionFilterGroups collisionFilterMask = isDynamic ? CollisionFilterGroups.AllFilter : CollisionFilterGroups.AllFilter ^ CollisionFilterGroups.StaticFilter;
//if (i==0||i>numLinks-2)
{
world.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask);//,2,1+2);
BulletSharp.Math.Vector4 color = colors[curColor];
curColor++;
curColor &= 3;
//guiHelper.createCollisionObjectGraphicsObject(col, color);
pMultiBody.GetLink(i).Collider = col;
}
}
return(pMultiBody);
}
//convert from Bullet
public static Quaternion Convert(BulletSharp.Math.Quaternion v)
{
return(new Quaternion(v.X, v.Y, v.Z, v.W));
}
public void ConvertURDF2BulletInternal(
URDFImporterInterface u2b,
MultiBodyCreationInterface creation,
URDF2BulletCachedData cache, int urdfLinkIndex,
Matrix parentTransformInWorldSpace,
GameObject parentGameObject,
MultiBodyDynamicsWorld world1,
bool createMultiBody, string pathPrefix,
bool enableConstraints,
ConvertURDFFlags flags = 0)
{
Matrix linkTransformInWorldSpace = Matrix.Identity;
int mbLinkIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
int urdfParentIndex = cache.getParentUrdfIndex(urdfLinkIndex);
int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex);
RigidBody parentRigidBody = null;
//b3Printf();
if (debugLevel >= BDebug.DebugType.Debug)
{
Debug.LogFormat("mb link index = {0}\n", mbLinkIndex);
}
Matrix parentLocalInertialFrame = Matrix.Identity;
float parentMass = (1);
BulletSharp.Math.Vector3 parentLocalInertiaDiagonal = new BulletSharp.Math.Vector3(1, 1, 1);
if (urdfParentIndex == -2)
{
if (debugLevel >= BDebug.DebugType.Debug)
{
Debug.LogFormat("root link has no parent\n");
}
}
else
{
if (debugLevel >= BDebug.DebugType.Debug)
{
Debug.LogFormat("urdf parent index = {0}", urdfParentIndex);
Debug.LogFormat("mb parent index = {0}", mbParentIndex);
}
parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex);
u2b.getMassAndInertia(urdfParentIndex, out parentMass, out parentLocalInertiaDiagonal, out parentLocalInertialFrame);
}
float mass = 0;
Matrix localInertialFrame = Matrix.Identity;
BulletSharp.Math.Vector3 localInertiaDiagonal = new BulletSharp.Math.Vector3(0, 0, 0);
u2b.getMassAndInertia(urdfLinkIndex, out mass, out localInertiaDiagonal, out localInertialFrame);
Matrix parent2joint = Matrix.Identity;
UrdfJointTypes jointType;
BulletSharp.Math.Vector3 jointAxisInJointSpace;
float jointLowerLimit;
float jointUpperLimit;
float jointDamping;
float jointFriction;
float jointMaxForce;
float jointMaxVelocity;
bool hasParentJoint = u2b.getJointInfo2(urdfLinkIndex, out parent2joint, out linkTransformInWorldSpace, out jointAxisInJointSpace, out jointType, out jointLowerLimit, out jointUpperLimit, out jointDamping, out jointFriction, out jointMaxForce, out jointMaxVelocity);
string linkName = u2b.getLinkName(urdfLinkIndex);
if ((flags & ConvertURDFFlags.CUF_USE_SDF) != 0)
{
Matrix tmp = new Matrix();
Matrix.Invert(ref parentTransformInWorldSpace, out tmp);
Matrix.Multiply(ref tmp, ref linkTransformInWorldSpace, out parent2joint);
}
else
{
if ((flags & ConvertURDFFlags.CUF_USE_MJCF) != 0)
{
linkTransformInWorldSpace = parentTransformInWorldSpace * linkTransformInWorldSpace;
}
else
{
linkTransformInWorldSpace = parentTransformInWorldSpace * parent2joint;
}
}
GameObject gameObject = new GameObject(linkName);
if (parentGameObject != null)
{
gameObject.transform.parent = parentGameObject.transform;
}
//--------------------
/*
* bool hasParentJoint = loader.getJointInfo2(linkIndex, out parent2joint, out linkTransformInWorldSpace, out jointAxisInJointSpace, out jointType, out jointLowerLimit, out jointUpperLimit, out jointDamping, out jointFriction, out jointMaxForce, out jointMaxVelocity);
* string linkName = loader.getLinkName(linkIndex);
*
* if ((flags & ConvertURDFFlags.CUF_USE_SDF) != 0)
* {
* Matrix tmp = parentTransformInWorldSpace.Inverse();
* Matrix.Multiply(ref tmp, ref linkTransformInWorldSpace, out parent2joint);
* }
* else
* {
* if ((flags & ConvertURDFFlags.CUF_USE_MJCF) != 0)
* {
* linkTransformInWorldSpace = parentTransformInWorldSpace * linkTransformInWorldSpace;
* }
* else
* {
* linkTransformInWorldSpace = parentTransformInWorldSpace * parent2joint;
* }
* }
*
* lgo.transform.position = linkTransformInWorldSpace.Origin.ToUnity();
*/
//--------------------
gameObject.transform.position = linkTransformInWorldSpace.Origin.ToUnity();
gameObject.transform.rotation = linkTransformInWorldSpace.Rotation.ToUnity();
BCollisionShape compoundShape = u2b.convertLinkCollisionShapes(urdfLinkIndex, pathPrefix, ref localInertialFrame, gameObject);
int graphicsIndex;
{
graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex, pathPrefix, ref localInertialFrame);
}
if (compoundShape != null)
{
UrdfMaterialColor matColor;
Color color2 = Color.red;
Color specular = new Color(0.5f, 0.5f, 0.5f);
if (u2b.getLinkColor2(urdfLinkIndex, out matColor))
{
color2 = matColor.m_rgbaColor;
specular = matColor.m_specularColor;
}
if (mass != 0)
{
if ((flags & ConvertURDFFlags.CUF_USE_URDF_INERTIA) == 0)
{
compoundShape.GetCollisionShape().CalculateLocalInertia(mass, out localInertiaDiagonal);
Debug.Assert(localInertiaDiagonal[0] < 1e10);
Debug.Assert(localInertiaDiagonal[1] < 1e10);
Debug.Assert(localInertiaDiagonal[2] < 1e10);
}
URDFLinkContactInfo contactInfo;
u2b.getLinkContactInfo(urdfLinkIndex, out contactInfo);
//temporary inertia scaling until we load inertia from URDF
if ((contactInfo.m_flags & URDF_LinkContactFlags.URDF_CONTACT_HAS_INERTIA_SCALING) != 0)
{
localInertiaDiagonal *= contactInfo.m_inertiaScaling;
}
}
RigidBody linkRigidBody = null;
Matrix inertialFrameInWorldSpace = linkTransformInWorldSpace * localInertialFrame;
if (!createMultiBody)
{
RigidBody body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape.GetCollisionShape());
linkRigidBody = body;
world1.AddRigidBody(body);
compoundShape.GetCollisionShape().UserIndex = (graphicsIndex);
URDFLinkContactInfo contactInfo;
u2b.getLinkContactInfo(urdfLinkIndex, out contactInfo);
ProcessContactParameters(contactInfo, body);
creation.createRigidBodyGraphicsInstance2(urdfLinkIndex, body, color2, specular, graphicsIndex);
cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape.GetCollisionShape(), ref localInertialFrame);
//untested: u2b.convertLinkVisualShapes2(linkIndex,urdfLinkIndex,pathPrefix,localInertialFrame,body);
}
else
{
if (cache.m_bulletMultiBody == null)
{
// creating base
bool canSleep = false;
bool isFixedBase = (mass == 0);//todo: figure out when base is fixed
int totalNumJoints = cache.m_totalNumJoints1;
//cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints, mass, localInertiaDiagonal, isFixedBase, canSleep);
BMultiBody bmm = cache.m_bulletMultiBody = gameObject.AddComponent <BMultiBody>();
bmm.fixedBase = isFixedBase;
bmm.canSleep = canSleep;
bmm.baseMass = mass;
//new btMultiBody(totalNumJoints, mass, localInertiaDiagonal, isFixedBase, canSleep);
//if ((flags & ConvertURDFFlags.CUF_USE_MJCF) != 0)
//{
// cache.m_bulletMultiBody.BaseWorldTransform = (linkTransformInWorldSpace);
//}
//cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape.GetCollisionShape(), ref localInertialFrame);
}
}
//create a joint if necessary
BMultiBodyLink bmbl = null;
if (hasParentJoint)
{
//====================
//btTransform offsetInA, offsetInB;
//offsetInA = parentLocalInertialFrame.inverse() * parent2joint;
//offsetInB = localInertialFrame.inverse();
//btQuaternion parentRotToThis = offsetInB.getRotation() * offsetInA.inverse().getRotation();
//=====================
Matrix offsetInA = new Matrix(), offsetInB = new Matrix();
Matrix.Invert(ref parentLocalInertialFrame, out offsetInA);
offsetInA = offsetInA * parent2joint;
Matrix.Invert(ref localInertialFrame, out offsetInB);
Matrix offsetInAInv = new Matrix();
Matrix.Invert(ref offsetInA, out offsetInAInv);
BulletSharp.Math.Quaternion parentRotToThis = offsetInB.GetRotation() * offsetInAInv.GetRotation();
Matrix tmp = new Matrix();
Matrix.Invert(ref parentTransformInWorldSpace, out tmp);
Matrix tmp2 = new Matrix();
Matrix.Invert(ref parent2joint, out tmp2);
Matrix tmp3 = new Matrix();
Matrix tmp4 = linkTransformInWorldSpace;
Matrix link2joint = new Matrix();
Matrix.Multiply(ref tmp, ref tmp2, out tmp3);
Matrix.Multiply(ref tmp3, ref tmp4, out link2joint);
if (debugLevel >= BDebug.DebugType.Debug)
{
Matrix linkTransformInWorldSpaceInv = new Matrix(), parentTransformInWorldSpaceInv = new Matrix();
Matrix.Invert(ref linkTransformInWorldSpace, out linkTransformInWorldSpaceInv);
Matrix.Invert(ref parentTransformInWorldSpace, out parentTransformInWorldSpaceInv);
Debug.Log("Creating link " + linkName + " offsetInA=" + offsetInA.Origin + " offsetInB=" + offsetInB.Origin + " parentLocalInertialFrame=" + parentLocalInertialFrame.Origin + " localInertialFrame=" + localInertialFrame.Origin + " localTrnaform=" +
" linkTransInWorldSpace=" + linkTransformInWorldSpace.Origin +
" linkTransInWorldSpaceInv=" + linkTransformInWorldSpaceInv.Origin +
" parentTransInWorldSpace=" + parentTransformInWorldSpace.Origin +
" parentTransInWorldSpaceInv=" + parentTransformInWorldSpaceInv.Origin +
" link2joint=" + link2joint.Origin +
" jointType=" + jointType);
}
bool disableParentCollision = true;
switch (jointType)
{
case UrdfJointTypes.URDFFloatingJoint:
case UrdfJointTypes.URDFPlanarJoint:
case UrdfJointTypes.URDFFixedJoint:
{
if ((jointType == UrdfJointTypes.URDFFloatingJoint) || (jointType == UrdfJointTypes.URDFPlanarJoint))
{
Debug.Log("Warning: joint unsupported, creating a fixed joint instead.");
}
//creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
if (createMultiBody)
{
//todo: adjust the center of mass transform and pivot axis properly
/*
* cache.m_bulletMultiBody.SetupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
* parentRotToThis, offsetInA.Origin, -offsetInB.Origin);
*/
bmbl = gameObject.AddComponent <BMultiBodyLink>();
bmbl.jointType = FeatherstoneJointType.Fixed;
bmbl.mass = mass;
bmbl.localPivotPosition = offsetInB.Origin.ToUnity();
}
else
{
//b3Printf("Fixed joint\n");
Debug.LogError("TODO Setup 6dof ");
/*
* Generic6DofSpring2Constraint dof6 = null;
*
* //backward compatibility
* if ((flags & ConvertURDFFlags.CUF_RESERVED) != 0)
* {
* dof6 = creation.createFixedJoint(urdfLinkIndex, parentRigidBody, linkRigidBody, offsetInA, offsetInB);
* }
* else
* {
* dof6 = creation.createFixedJoint(urdfLinkIndex, linkRigidBody, parentRigidBody, offsetInB, offsetInA);
* }
* if (enableConstraints)
* world1.AddConstraint(dof6, true);
*/
}
break;
}
case UrdfJointTypes.URDFContinuousJoint:
case UrdfJointTypes.URDFRevoluteJoint:
{
//creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
if (createMultiBody)
{
//cache.m_bulletMultiBody.SetupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
// parentRotToThis, (offsetInB.GetRotation().Rotate(jointAxisInJointSpace)), offsetInA.Origin,//parent2joint.getOrigin(),
// -offsetInB.Origin,
// disableParentCollision);
bmbl = gameObject.AddComponent <BMultiBodyLink>();
bmbl.jointType = FeatherstoneJointType.Revolute;
bmbl.mass = mass;
bmbl.localPivotPosition = offsetInB.Origin.ToUnity();
bmbl.rotationAxis = offsetInB.Rotation.Rotate(jointAxisInJointSpace).ToUnity();
if (jointType == UrdfJointTypes.URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit)
{
//string name = u2b.getLinkName(urdfLinkIndex);
//printf("create btMultiBodyJointLimitConstraint for revolute link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit, jointUpperLimit);
BMultiBodyJointLimitConstraint mbc = gameObject.AddComponent <BMultiBodyJointLimitConstraint>();
mbc.m_jointLowerLimit = jointLowerLimit;
mbc.m_jointUpperLimit = jointUpperLimit;
}
/*
* Debug.Log("=========== Creating joint for: " + gameObject.name);
* Debug.Log("parentRotateToThis: " + parentRotToThis.ToUnity().eulerAngles);
* Debug.Log("rotationAxis: " + bmbl.rotationAxis);
* Debug.Log("offsetInA: " + offsetInA.Origin);
* Debug.Log("negOffsetInB: " + -offsetInB.Origin);
*/
}
else
{
Generic6DofSpring2Constraint dof6 = null;
//backwards compatibility
if ((flags & ConvertURDFFlags.CUF_RESERVED) != 0)
{
dof6 = creation.createRevoluteJoint(urdfLinkIndex, parentRigidBody, linkRigidBody, offsetInA, offsetInB, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit);
}
else
{
dof6 = creation.createRevoluteJoint(urdfLinkIndex, linkRigidBody, parentRigidBody, offsetInB, offsetInA, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit);
}
if (enableConstraints)
{
world1.AddConstraint(dof6, true);
}
//b3Printf("Revolute/Continuous joint\n");
}
break;
}
case UrdfJointTypes.URDFPrismaticJoint:
{
//creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
if (createMultiBody)
{
//cache.m_bulletMultiBody.SetupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
// parentRotToThis, (offsetInB.GetRotation().Rotate(jointAxisInJointSpace)), offsetInA.Origin,//parent2joint.getOrigin(),
// -offsetInB.Origin,
// disableParentCollision);
bmbl = gameObject.AddComponent <BMultiBodyLink>();
bmbl.jointType = FeatherstoneJointType.Prismatic;
bmbl.mass = mass;
bmbl.localPivotPosition = offsetInB.Origin.ToUnity();
bmbl.rotationAxis = offsetInB.Rotation.Rotate(jointAxisInJointSpace).ToUnity();
if (jointLowerLimit <= jointUpperLimit)
{
//string name = u2b.getLinkName(urdfLinkIndex);
//printf("create btMultiBodyJointLimitConstraint for prismatic link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit,jointUpperLimit);
BMultiBodyJointLimitConstraint mbc = gameObject.AddComponent <BMultiBodyJointLimitConstraint>();
mbc.m_jointLowerLimit = jointLowerLimit;
mbc.m_jointUpperLimit = jointUpperLimit;
}
//printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit);
}
else
{
Generic6DofSpring2Constraint dof6 = creation.createPrismaticJoint(urdfLinkIndex, parentRigidBody, linkRigidBody, offsetInA, offsetInB, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit);
if (enableConstraints)
{
world1.AddConstraint(dof6, true);
}
//b3Printf("Prismatic\n");
}
break;
}
default:
{
//b3Printf("Error: unsupported joint type in URDF (%d)\n", jointType);
Debug.Assert(false);
break;
}
}
}
if (createMultiBody)
{
if (bmbl != null)
{
bmbl.jointDamping = jointDamping;
bmbl.jointFriction = jointFriction;
//bmbl.jointLowerLimit = jointLowerLimit;
//bmbl.jointUpperLimit = jointUpperLimit;
//bmbl.jointMaxForce = jointMaxForce;
//bmbl.jointMaxVelocity = jointMaxVelocity;
}
{
/*
* MultiBodyLinkCollider col = creation.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody);
*
* compoundShape.GetCollisionShape().UserIndex = (graphicsIndex);
*
* col.CollisionShape = (compoundShape.GetCollisionShape());
*
* Matrix tr = Matrix.Identity;
*
* tr = linkTransformInWorldSpace;
* //if we don't set the initial pose of the btCollisionObject, the simulator will do this
* //when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
*
* col.WorldTransform = (tr);
*
* //base and fixed? . static, otherwise flag as dynamic
* bool isDynamic = (mbLinkIndex < 0 && cache.m_bulletMultiBody.HasFixedBase) ? false : true;
* CollisionFilterGroups collisionFilterGroup = isDynamic ? (CollisionFilterGroups.DefaultFilter) : (CollisionFilterGroups.StaticFilter);
* CollisionFilterGroups collisionFilterMask = isDynamic ? (CollisionFilterGroups.AllFilter) : (CollisionFilterGroups.AllFilter ^ CollisionFilterGroups.StaticFilter);
*
* CollisionFilterGroups colGroup = 0, colMask = 0;
* UrdfCollisionFlags collisionFlags = u2b.getCollisionGroupAndMask(urdfLinkIndex, out colGroup, out colMask);
* if ((collisionFlags & UrdfCollisionFlags.URDF_HAS_COLLISION_GROUP) != 0)
* {
* collisionFilterGroup = colGroup;
* }
* if ((collisionFlags & UrdfCollisionFlags.URDF_HAS_COLLISION_MASK) != 0)
* {
* collisionFilterMask = colMask;
* }
* world1.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask);
*
* color2 = Color.red;//(0.0,0.0,0.5);
* Color specularColor = new Color(1, 1, 1);
* UrdfMaterialColor matCol;
* if (u2b.getLinkColor2(urdfLinkIndex, out matCol))
* {
* color2 = matCol.m_rgbaColor;
* specularColor = matCol.m_specularColor;
* }
* {
*
*
* creation.createCollisionObjectGraphicsInstance2(urdfLinkIndex, col, color2, specularColor);
* }
* {
*
*
* u2b.convertLinkVisualShapes2(mbLinkIndex, urdfLinkIndex, pathPrefix, ref localInertialFrame, col, u2b.getBodyUniqueId());
* }
* URDFLinkContactInfo contactInfo;
* u2b.getLinkContactInfo(urdfLinkIndex, out contactInfo);
*
*
* ProcessContactParameters(contactInfo, col);
*
* if (mbLinkIndex >= 0) //???? double-check +/- 1
* {
* cache.m_bulletMultiBody.GetLink(mbLinkIndex).Collider = col;
* if ((flags & ConvertURDFFlags.CUF_USE_SELF_COLLISION_EXCLUDE_PARENT) != 0)
* {
* cache.m_bulletMultiBody.GetLink(mbLinkIndex).Flags |= (int)btMultiBodyLinkFlags.BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
* }
* if ((flags & ConvertURDFFlags.CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS) != 0)
* {
* cache.m_bulletMultiBody.GetLink(mbLinkIndex).Flags |= (int)btMultiBodyLinkFlags.BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION;
* }
* }
* else
* {
* cache.m_bulletMultiBody.BaseCollider = (col);
* }
*/
}
}
else
{
//u2b.convertLinkVisualShapes2(urdfLinkIndex,urdfIndex,pathPrefix,localInertialFrame,compoundShape);
}
}
List <int> urdfChildIndices = new List <int>();
u2b.getLinkChildIndices(urdfLinkIndex, urdfChildIndices);
int numChildren = urdfChildIndices.Count;
for (int i = 0; i < numChildren; i++)
{
int urdfChildLinkIndex = urdfChildIndices[i];
ConvertURDF2BulletInternal(u2b, creation, cache, urdfChildLinkIndex, linkTransformInWorldSpace, gameObject, world1, createMultiBody, pathPrefix, enableConstraints, flags);
}
}
/// <summary>
/// Get Quaternion rotation from BulletSharp Matrix4x4
/// </summary>
/// <param name="bMatrix4x4">Matrix from which to extract a rotatio</param>
/// <returns></returns>
public static Quaternion GetOrientation(this Matrix bMatrix4x4)
{
//Scaling is the length of the rows.
Vector3 scale;
scale.X = (float)Math.Sqrt((bMatrix4x4.M11 * bMatrix4x4.M11) + (bMatrix4x4.M12 * bMatrix4x4.M12) + (bMatrix4x4.M13 * bMatrix4x4.M13));
scale.Y = (float)Math.Sqrt((bMatrix4x4.M21 * bMatrix4x4.M21) + (bMatrix4x4.M22 * bMatrix4x4.M22) + (bMatrix4x4.M23 * bMatrix4x4.M23));
scale.Z = (float)Math.Sqrt((bMatrix4x4.M31 * bMatrix4x4.M31) + (bMatrix4x4.M32 * bMatrix4x4.M32) + (bMatrix4x4.M33 * bMatrix4x4.M33));
//Divide out scaling to get rotation
var mm11 = bMatrix4x4.M11 / scale.X;
var mm12 = bMatrix4x4.M12 / scale.X;
var mm13 = bMatrix4x4.M13 / scale.X;
var mm21 = bMatrix4x4.M21 / scale.Y;
var mm22 = bMatrix4x4.M22 / scale.Y;
var mm23 = bMatrix4x4.M23 / scale.Y;
var mm31 = bMatrix4x4.M31 / scale.Z;
var mm32 = bMatrix4x4.M32 / scale.Z;
var mm33 = bMatrix4x4.M33 / scale.Z;
float sqrt;
float half;
var trace = mm11 + mm22 + mm33;
var result = new Quaternion();
if (trace > 0.0f)
{
sqrt = Mathf.Sqrt(trace + 1.0f);
result.W = sqrt * 0.5f;
sqrt = 0.5f / sqrt;
result.X = (mm23 - mm32) * sqrt;
result.Y = (mm31 - mm13) * sqrt;
result.Z = (mm12 - mm21) * sqrt;
}
else if ((mm11 >= mm22) && (mm11 >= mm33))
{
sqrt = Mathf.Sqrt(1.0f + mm11 - mm22 - mm33);
half = 0.5f / sqrt;
result.X = 0.5f * sqrt;
result.Y = (mm12 + mm21) * half;
result.Z = (mm13 + mm31) * half;
result.W = (mm23 - mm32) * half;
}
else if (mm22 > mm33)
{
sqrt = Mathf.Sqrt(1.0f + mm22 - mm11 - mm33);
half = 0.5f / sqrt;
result.X = (mm21 + mm12) * half;
result.Y = 0.5f * sqrt;
result.Z = (mm32 + mm23) * half;
result.W = (mm31 - mm13) * half;
}
else
{
sqrt = Mathf.Sqrt(1.0f + mm33 - mm11 - mm22);
half = 0.5f / sqrt;
result.X = (mm31 + mm13) * half;
result.Y = (mm32 + mm23) * half;
result.Z = 0.5f * sqrt;
result.W = (mm12 - mm21) * half;
}
return(result);
}
/// <summary>
/// Convert a BulletSharp Quaternion to Unity
/// </summary>
/// <param name="q">Quaternion to be converted</param>
/// <returns></returns>
public static UnityEngine.Quaternion ToUnity(this Quaternion q)
{
return(new UnityEngine.Quaternion(q.X, q.Y, q.Z, q.W));
}
