private RigidBody CreateRigidBody(GameObject gameObject, Link link)
{
var rb = GetOrCreateComponent <RigidBody>(gameObject);
GetOrCreateComponent <ElementComponent>(gameObject).SetElement(link);
if (link.IsStatic)
{
rb.MotionControl = agx.RigidBody.MotionControl.STATIC;
return(rb);
}
// Note: When <inertial> isn't defined the collision shapes defines
// the mass properties.
// <inertial> defined with required <mass> and <inertia>. Create
// a native rigid body with the given properties and read the
// values back to our rigid body component.
if (link.Inertial != null)
{
var native = new agx.RigidBody();
native.getMassProperties().setMass(link.Inertial.Mass, false);
// Inertia tensor is given in the inertia frame. The rotation of the
// CM frame can't be applied to the CM frame so we transform the inertia
// CM frame and rotate the game object.
var rotationMatrix = link.Inertial.Rpy.RadEulerToRotationMatrix();
var inertia3x3 = (agx.Matrix3x3)link.Inertial.Inertia;
inertia3x3 = rotationMatrix.transpose().Multiply(inertia3x3).Multiply(rotationMatrix);
native.getMassProperties().setInertiaTensor(new agx.SPDMatrix3x3(inertia3x3));
native.getCmFrame().setLocalTranslate(link.Inertial.Xyz.ToVec3());
rb.RestoreLocalDataFrom(native);
}
return(rb);
}
public AgX_Frame(Guid guid, string shape, Vector3[] vertices, Vector2[] uvs, int[] triangles, double size, Vector3 pos, Quaternion rot, double mass, bool isStatic, string materialName)
{
this.guid = guid;
this.shape = shape;
this.size = size;
this.materialName = materialName;
var tri = new agxCollide.Trimesh(Operations.ToAgxVec3Vector(vertices), Operations.ToAgxIntVector(triangles), "stdFrame");
///Creates a geometry
var dynamicRBGeometry = new agxCollide.Geometry();
dynamicRBGeometry.add(tri);
dynamicRBGeometry.setMaterial(new agx.Material(materialName));
agx_Object = new agx.RigidBody();
///Adds selected geometry to the rigidbody
agx_Object.add(dynamicRBGeometry);
agx_Object.setLocalPosition(Operations.ToAgxVec3(pos)); ///AgX
agx_Object.setLocalRotation(new agx.Quat(rot.x, rot.y, rot.z, rot.w)); ///AgX
agx_Object.getMassProperties().setMass(mass);
if (isStatic)
{
agx_Object.setMotionControl(agx.RigidBody.MotionControl.STATIC);
}
AddToAssembly();
}
public AgX_Primitive(Guid guid, string shape, Vector3 pos, Quaternion rot, Vector3 size, double mass, string materialName)
{
this.guid = guid;
this.shape = shape;
this.size = size;
this.materialName = materialName;
var dynamicRBGeometry = new agxCollide.Geometry();
switch (shape)
{
case "Box": dynamicRBGeometry.add(new agxCollide.Box(Operations.ToAgxVec3(this.size))); break;
case "Sphere": dynamicRBGeometry.add(new agxCollide.Sphere((this.size.x + this.size.y + this.size.z) / 3)); break;
}
dynamicRBGeometry.setMaterial(new agx.Material(materialName));
agx_Object = new agx.RigidBody();
agx_Object.add(dynamicRBGeometry);
agx_Object.setLocalPosition(Operations.ToAgxVec3(pos)); ///AgX
agx_Object.setLocalRotation(new agx.Quat(rot.x, rot.y, rot.z, rot.w)); ///AgX
agx_Object.getMassProperties().setMass(mass);
AddToSim();
}
protected override bool Initialize()
{
m_transform = transform;
Shapes = GetShapes();
HasArticulatedRoot = GetArticulatedRoot() != null &&
GetArticulatedRoot().enabled;
VerifyConfiguration();
m_rb = new agx.RigidBody();
m_rb.setName(name);
m_rb.setEnable(IsEnabled);
m_rb.getMassProperties().setAutoGenerateMask(0u);
SyncNativeTransform(m_rb);
SyncShapes();
GetSimulation().add(m_rb);
UpdateMassProperties();
if (IsEnabled)
{
HandleUpdateCallbacks(true);
}
return(true);
}
/// <summary>
/// Reads values from native instance.
/// </summary>
/// <param name="native">Source native instance.</param>
public void RestoreLocalDataFrom(agx.RigidBody native)
{
Mass.UserValue = Convert.ToSingle(native.getMassProperties().getMass());
var nativeInertia = native.getMassProperties().getInertiaTensor();
InertiaDiagonal.UserValue = nativeInertia.getDiagonal().ToVector3();
InertiaOffDiagonal.UserValue = GetNativeOffDiagonal(nativeInertia).ToVector3();
CenterOfMassOffset.UserValue = native.getCmFrame().getLocalTranslate().ToHandedVector3();
Mass.UseDefault = false;
InertiaDiagonal.UseDefault = false;
InertiaOffDiagonal.UseDefault = false;
CenterOfMassOffset.UseDefault = false;
}
public AgX_Frame(Guid guid, string shape, Vector3[] vertices, Vector2[] uvs, int[] triangles, double size, Vector3 pos, Quaternion rot, double mass, bool isStatic, string materialName)
{
this.guid = guid;
this.shape = shape;
this.size = size;
this.materialName = materialName;
//scale by 2, to fit unity.
/* Vector3[] tmp_verts = vertices;
* for (int i = 0; i < tmp_verts.Length; i++)
* {
* tmp_verts[i].x *= 2;
* tmp_verts[i].y *= 2;
* tmp_verts[i].z *= 2;
* }*/
var tri = new agxCollide.Trimesh(Operations.ToAgxVec3Vector(vertices), Operations.ToAgxIntVector(triangles), "stdFrame");
///Creates a geometry
var dynamicRBGeometry = new agxCollide.Geometry();
dynamicRBGeometry.add(tri);
dynamicRBGeometry.setMaterial(new agx.Material(materialName));
///Creates the selected shape
/*switch (shape)
* {
* case "Box": dynamicRBGeometry.add(new agxCollide.Box(Operations.ToAgxVec3(size))); break;
* case "Sphere": dynamicRBGeometry.add(new agxCollide.Sphere((size.x + size.y + size.z) / 3)); break;
* }*/
agx_Object = new agx.RigidBody();
///Adds selected geometry to the rigidbody
agx_Object.add(dynamicRBGeometry);
agx_Object.setLocalPosition(Operations.ToAgxVec3(pos));///AgX
//var y = new agx.EulerAngles(Operations.ToAgxVec3(rot));
//UnityEngine.Debug.Log("x: " + y.x + ", y: " + y.y + ", z: " + y.z);
agx_Object.setLocalRotation(new agx.Quat(rot.x, rot.y, rot.z, rot.w));///AgX
//agx_Object.setLocalRotation(new agx.EulerAngles(Operations.ToAgxVec3(rot)));///AgX
//UnityEngine.Debug.Log("x: " +agx_Object.getLocalPosition().x + ", y: " + agx_Object.getLocalPosition().y + ", z: " + agx_Object.getLocalPosition().z);
agx_Object.getMassProperties().setMass(mass);
if (isStatic)
{
agx_Object.setMotionControl(agx.RigidBody.MotionControl.STATIC);
}
AddToSim();
}
/// <summary>
/// Callback from RigidBody when mass properties has been calculated for a native instance.
/// </summary>
/// <param name="nativeRb">Native rigid body instance.</param>
public void SetDefaultCalculated(agx.RigidBody nativeRb)
{
if (nativeRb == null)
{
return;
}
Mass.DefaultValue = Convert.ToSingle(nativeRb.getMassProperties().getMass());
float inertiaScale = 1.0f;
if (!Mass.UseDefault)
{
inertiaScale = Mass.UserValue / Mass.DefaultValue;
}
InertiaDiagonal.DefaultValue = inertiaScale * nativeRb.getMassProperties().getPrincipalInertiae().ToVector3();
}
public static agx.RigidBody InstantiateTemplate(RigidBody template, Shape[] shapes)
{
if (template == null)
{
return(null);
}
var native = new agx.RigidBody(template.name);
foreach (var shape in shapes)
{
var geometry = shape.CreateTemporaryNative();
geometry.setEnable(shape.IsEnabled);
if (shape.Material != null)
{
geometry.setMaterial(shape.Material.GetInitialized <ShapeMaterial>().Native);
}
native.add(geometry, shape.GetNativeRigidBodyOffset(template));
}
template.SyncNativeTransform(native);
// MassProperties (synchronization below) wont write any data if UseDefault = true.
native.getMassProperties().setAutoGenerateMask((uint)agx.MassProperties.AutoGenerateFlags.AUTO_GENERATE_ALL);
native.updateMassProperties();
template.MassProperties.SetDefaultCalculated(native);
native.getMassProperties().setAutoGenerateMask(0u);
var prevNative = template.m_rb;
try {
template.m_rb = native;
PropertySynchronizer.Synchronize(template);
PropertySynchronizer.Synchronize(template.MassProperties);
}
finally {
template.m_rb = prevNative;
}
return(native);
}
/// <summary>
/// Callback from RigidBody when mass properties has been calculated for a native instance.
/// </summary>
/// <param name="nativeRb">Native rigid body instance.</param>
public void SetDefaultCalculated(agx.RigidBody nativeRb)
{
if (nativeRb == null)
{
return;
}
Mass.DefaultValue = Convert.ToSingle(nativeRb.getMassProperties().getMass());
CenterOfMassOffset.DefaultValue = nativeRb.getCmFrame().getLocalTranslate().ToHandedVector3();
float inertiaScale = 1.0f;
if (!Mass.UseDefault)
{
inertiaScale = Mass.UserValue / Mass.DefaultValue;
}
InertiaDiagonal.DefaultValue = inertiaScale * nativeRb.getMassProperties().getPrincipalInertiae().ToVector3();
InertiaOffDiagonal.DefaultValue = inertiaScale * GetNativeOffDiagonal(nativeRb.getMassProperties().getInertiaTensor()).ToVector3();
}
public void RestoreLocalDataFrom(agx.RigidBody native)
{
if (native == null)
{
throw new ArgumentNullException("native", "Native object is null.");
}
MassProperties.RestoreLocalDataFrom(native.getMassProperties());
enabled = native.getEnable();
MotionControl = native.getMotionControl();
HandleAsParticle = native.getHandleAsParticle();
LinearVelocity = native.getVelocity().ToHandedVector3();
LinearVelocityDamping = native.getLinearVelocityDamping().ToHandedVector3();
AngularVelocity = native.getAngularVelocity().ToHandedVector3();
AngularVelocityDamping = native.getAngularVelocityDamping().ToHandedVector3();
}
public AgX_Primitive(Guid guid, string shape, Vector3 pos, Quaternion rot, Vector3 size, double mass, string materialName, bool isStatic, bool AddToRobot)
{
this.guid = guid;
this.shape = shape;
this.size = size;
this.materialName = materialName;
var dynamicRBGeometry = new agxCollide.Geometry();
switch (this.shape)
{
case "Box": dynamicRBGeometry.add(new agxCollide.Box(Operations.ToAgxVec3(this.size))); break;
case "Sphere": dynamicRBGeometry.add(new agxCollide.Sphere((this.size.x + this.size.y + this.size.z) / 3)); break;
default: dynamicRBGeometry.add(new agxCollide.Box(Operations.ToAgxVec3(this.size))); break;
}
dynamicRBGeometry.setMaterial(new agx.Material(this.materialName));
agx_Object = new agx.RigidBody();
agx_Object.add(dynamicRBGeometry);
agx_Object.setLocalPosition(Operations.ToAgxVec3(pos)); ///AgX
agx_Object.setLocalRotation(new agx.Quat(rot.x, rot.y, rot.z, rot.w)); ///AgX
agx_Object.getMassProperties().setMass(mass);
if (isStatic)
{
agx_Object.setMotionControl(agx.RigidBody.MotionControl.STATIC);
}
if (AddToRobot)
{
AddToAssembly();
}
else
{
Agx_Simulation.sim_Instance.add(agx_Object);
}
}
public AgX_Sensor(Guid guid, string materialName, Vector3 pos, Vector3 scale, double mass)
{
this.guid = guid;
this.scale = scale;
var dynamicRBGeometry = new agxCollide.Geometry();///AgX
dynamicRBGeometry.add(new agxCollide.Box(Operations.ToAgxVec3(scale)));
dynamicRBGeometry.setMaterial(new agx.Material(materialName));
agx_Object = new agx.RigidBody();
agx_Object.add(dynamicRBGeometry);
agx_Object.setLocalPosition(Operations.ToAgxVec3(pos));
agx_Object.getMassProperties().setMass(mass);
Agx_Simulation.sim_Instance.add(agx_Object);
}
public double Get_Mass()
{
return((double)agx_Object.getMassProperties().getMass());
}
