private void updateRigidBodies(ISource source)
{
// Early-out if there are no rigid bodies
if (_rigidBodies.Count == 0)
{
return;
}
int numSleeping = 0;
for (int i = 0; i < _rigidBodies.Count; i++)
{
RigidBodyData data = _rigidBodies.Values[i];
// Update data from snapshot update
source.Update(ref data);
if (data.Updated && data.MotionType == MotionType.Sleeping)
{
numSleeping++;
}
}
_areAllRigidBodiesSleeping = numSleeping == _rigidBodies.Count;
}
protected override void OnLoad(EventArgs e)
{
base.OnLoad(e);
ScreenContext.CameraMotionProvider = new BasicCameraControllerMotionProvider(this, this);
BasicGrid grid = new BasicGrid();
grid.Load(RenderContext);
WorldSpace.AddResource(grid);
bulletPhysics = new BulletPhysics(new Vector3(0, -9.8f, 0f));
ground = bulletPhysics.CreatePlane(0, Matrix.Identity);
ball_Model = MMDModel.OpenLoad("1.pmx", RenderContext);
ball_Model2 = MMDModel.OpenLoad("1.pmx", RenderContext);
RigidBodyData data = ball_Model.Model.RigidBodyList.RigidBodies[0];
ball = bulletPhysics.CreateSphere(data.Size.X, Matrix.Translation(-15, 12f, 0), 1f, 1f, 0f);
ball2 = bulletPhysics.CreateSphere(data.Size.X, Matrix.Translation(15, 12f, 0), 1f, 1f, 0f);
rigid2model = GetModelWorldFromRigid();
WorldSpace.AddResource(ball_Model);
WorldSpace.AddResource(ball_Model2);
CreateWalls();
ball.ApplyCentralImpulse(new Vector3(2, 0.01f, 0f));
ball2.ApplyCentralImpulse(new Vector3(-2, 0, 0));
brush = SpriteBatch.CreateSolidColorBrush(Color.Brown);
format = SpriteBatch.CreateTextformat("Meiriyo", 30);
format.Format.ParagraphAlignment = ParagraphAlignment.Center;
format.Format.TextAlignment = TextAlignment.Center;
timer1.Start();
}
public void Update(RigidBodyData body)
{
// if (body.isParticle)
// {
// body.inertia = TSMatrix.Zero;
// body.invInertia = body.invInertiaWorld = TSMatrix.Zero;
// body.invOrientation = body.orientation = TSMatrix.Identity;
// body.boundingBox = body.shape.boundingBox;
// TSVector.Add(ref body.boundingBox.min, ref body.position, out body.boundingBox.min);
// TSVector.Add(ref body.boundingBox.max, ref body.position, out body.boundingBox.max);
//
// body.angularVelocity.MakeZero();
// }
// else
// {
// // Given: Orientation, Inertia
// TSMatrix.Transpose(ref body.orientation, out body.invOrientation);
// body.Shape.GetBoundingBox(ref body.orientation, out body.boundingBox);
// TSVector.Add(ref body.boundingBox.min, ref body.position, out body.boundingBox.min);
// TSVector.Add(ref body.boundingBox.max, ref body.position, out body.boundingBox.max);
//
//
// if (!body.isStatic)
// {
// TSMatrix.Multiply(ref body.invOrientation, ref body.invInertia, out body.invInertiaWorld);
// TSMatrix.Multiply(ref body.invInertiaWorld, ref body.orientation, out body.invInertiaWorld);
// }
// }
}
//THE MOST IMPORTANT FUNCTION: to define the UniForm of broadcasting optitrack data.
/**
* Modified by daaqing & zsyzgu from Optitrack Standard Program.
*
*
*
*
*
**/
private void broadcast()
{
server.Send("begin");
FrameOfMocapData frame = m_FrameOfData;
data += "Frame\n";
data += frame.fTimestamp + "\n";
data += DateTime.Now.TimeOfDay.ToString(); // 20:33:50.7187500
for (int i = 0; i < frame.nRigidBodies; ++i)
{
RigidBodyData rb = frame.RigidBodies[i];
float[] quat = new float[4] {
rb.qx, rb.qy, rb.qz, rb.qw
};
float[] eulers = m_NatNet.QuatToEuler(quat, (int)NATEulerOrder.NAT_XYZr);
int id = rb.ID;
double x = -rb.x * 0.5;
double y = rb.y * 0.5;
double z = rb.z * 0.5;
double rx = RadiansToDegrees(eulers[0]);
double ry = RadiansToDegrees(eulers[1]);
double rz = -RadiansToDegrees(eulers[2]);
server.Send("rb " + id + " " + x + " " + y + " " + z + " " + rx + " " + ry + " " + rz);
data += "rb " + id + " " + rb.Tracked + " " + x + " " + y + " " + z + " " + rx + " " + ry + " " + rz + "\n";
}
for (int i = 0; i < frame.nOtherMarkers; i++)
{
Marker m = frame.OtherMarkers[i];
data += "x " + m.x + " y " + m.y + " z " + m.z + "\n";
}
data += "\n";
server.Send("end");
}
private void OnOptiTrackFrameReady(FrameOfMocapData data, NatNetClientML client)
{
if (data != null)
{
List <Tuple <RigidBody, RigidBodyData> > rigidBodyInfo = new List <Tuple <RigidBody, RigidBodyData> >();
for (int i = 0; i < data.nRigidBodies; i++)
{
RigidBodyData rigidBodyData = data.RigidBodies[i];
int id = rigidBodyData.ID;
RigidBody rigidBody = GetRigidBody(id);
if (rigidBody != null)
{
rigidBodyInfo.Add(new Tuple <RigidBody, RigidBodyData>(rigidBody, rigidBodyData));
}
}
List <Marker> markerInfo = new List <Marker>();
for (int i = 0; i < data.nOtherMarkers; i++)
{
if (data.OtherMarkers[i].ID == -1)
{
markerInfo.Add(data.OtherMarkers[i]);
}
}
TrackingUpdated?.Invoke(this, new OptiTrackTrackingEventArgs(
rigidBodyInfo, markerInfo, m_natNetClient, data.fTimestamp));
}
}
internal bool UpdateModel(OptiTrackTrackingEventArgs e)
{
bool armUpdated = false;
RigidBodyData forearmData = e[Constants.ForearmRigidBodyName];
RigidBodyData wristData = e[Constants.WristRigidBodyName];
if (forearmData != null &&
wristData != null)
{
double millimeterFactor = 1000.0;
m_forearmPosition = new Point3D(forearmData.x * millimeterFactor,
forearmData.y * millimeterFactor, forearmData.z * millimeterFactor);
m_forearmOrientation = new Quaternion(forearmData.qx,
forearmData.qy, forearmData.qz, forearmData.qw);
m_wristPosition = new Point3D(wristData.x * millimeterFactor,
wristData.y * millimeterFactor, wristData.z * millimeterFactor);
m_wristOrientation = new Quaternion(wristData.qx,
wristData.qy, wristData.qz, wristData.qw);
Application.Current.Dispatcher.Invoke(
new Action(delegate()
{
Update();
}));
armUpdated = true;
}
return(armUpdated);
}
internal bool UpdateModel(OptiTrackTrackingEventArgs e)
{
bool fingerUpdated = false;
RigidBodyData fingertipData = e[Constants.FingerRigidBodyName];
if (fingertipData != null)
{
double millimeterFactor = 1000.0;
m_fingertipPosition = new Point3D(fingertipData.x * millimeterFactor,
fingertipData.y * millimeterFactor, fingertipData.z * millimeterFactor);
m_fingertipOrientation = new Quaternion(fingertipData.qx,
fingertipData.qy, fingertipData.qz, fingertipData.qw);
Application.Current.Dispatcher.Invoke(
new Action(delegate()
{
Update();
}));
fingerUpdated = true;
}
return(fingerUpdated);
}
public override void OnStart()
{
base.OnStart();
_bodyData = _actor.GetData <RigidBodyData>();
_bodyData.body.velocity = Vector2.up * speed;
}
public void Update(ref RigidBodyData entry)
{
while (_nextIndex < _next.Transforms.Count &&
_next.Transforms[_nextIndex].RigidBodyId.CompareTo(entry.RigidBodyId) < 0)
{
_nextIndex++;
}
while (_prevIndex < _prev.Transforms.Count &&
_prev.Transforms[_prevIndex].RigidBodyId.CompareTo(entry.RigidBodyId) < 0)
{
_prevIndex++;
}
if (_nextIndex >= _next.Transforms.Count ||
_next.Transforms[_nextIndex].RigidBodyId != entry.RigidBodyId)
{
// velocity is zero
entry.SetZeroVelocities();
if (entry.MotionType == MotionType.Sleeping)
{
entry.Time = _timestamp;
entry.Updated = true;
}
else
{
entry.Updated = false;
}
return;
}
if (_prevIndex < _prev.Transforms.Count &&
_prev.Transforms[_prevIndex].RigidBodyId == _next.Transforms[_nextIndex].RigidBodyId)
{
RigidBodyTransform t = new RigidBodyTransform();
{
t.Lerp(_prev.Transforms[_prevIndex].Transform, _next.Transforms[_nextIndex].Transform, _frac);
}
entry.Transform = t;
entry.SetZeroVelocities();
// estimate velocity
float DT = _next.Time - _prev.Time;
var prevT = _prev.Transforms[_prevIndex].Transform;
var currentT = _next.Transforms[_nextIndex].Transform;
entry.SetVelocitiesFromTransformsDiff(DT, currentT, prevT);
}
else
{
entry.Transform = _next.Transforms[_nextIndex].Transform;
entry.SetZeroVelocities();
}
entry.Time = _timestamp;
entry.MotionType = _next.Transforms[_nextIndex].MotionType;
entry.Updated = true;
}
private Matrix GetModelWorldFromRigid()
{
RigidBodyData mmdRigidBody = ball_Model.Model.RigidBodyList.RigidBodies[0];
Vector3 pos = mmdRigidBody.Position; // モデルからみた剛体の位置
Vector3 rot = mmdRigidBody.Rotation; // モデルからみた剛体の回転
Matrix rigid_world_from_model = Matrix.RotationYawPitchRoll(rot.Y, rot.X, rot.Z) * Matrix.Translation(pos);
// モデルからみた剛体のワールド変換行列
return(Matrix.Invert(rigid_world_from_model));
}
public void Update(ref RigidBodyData entry)
{
if (_prevSnapshot != null)
{
while (_prevIndex < _prevSnapshot.Transforms.Count &&
_prevSnapshot.Transforms[_prevIndex].RigidBodyId.CompareTo(entry.RigidBodyId) < 0)
{
_prevIndex++;
}
}
while (_iteratorIndex < _snapshot.Transforms.Count &&
_snapshot.Transforms[_iteratorIndex].RigidBodyId.CompareTo(entry.RigidBodyId) < 0)
{
_iteratorIndex++;
}
//Debug.Log(" SnapshotSource Update cI:" + _iteratorIndex + " pI:" + _prevIndex +
// " DT=" + (_snapshot.Time - _prevSnapshot.Time));
if (_iteratorIndex < _snapshot.Transforms.Count &&
_snapshot.Transforms[_iteratorIndex].RigidBodyId == entry.RigidBodyId)
{
var rb = _snapshot.Transforms[_iteratorIndex];
entry.MotionType = rb.MotionType;
entry.Transform = rb.Transform;
entry.Time = _snapshot.Time;
entry.SetZeroVelocities();
if (_prevSnapshot != null)
{
if (_prevIndex < _prevSnapshot.Transforms.Count &&
_prevSnapshot.Transforms[_prevIndex].RigidBodyId == entry.RigidBodyId)
{
// estimate velocity
float DT = _snapshot.Time - _prevSnapshot.Time;
var prevRB = _prevSnapshot.Transforms[_prevIndex].Transform;
entry.SetVelocitiesFromTransformsDiff(DT, rb.Transform, prevRB);
}
}
entry.Updated = true;
}
else if (entry.MotionType == MotionType.Sleeping)
{
entry.Time = _snapshot.Time;
entry.Updated = true;
entry.SetZeroVelocities();
}
else
{
entry.Updated = false;
}
}
/// <summary>
/// 剛体データのテンポラリ
/// </summary>
/// <param name="rigidBodyData">剛体データ</param>
public TempRigidBodyData(RigidBodyData rigidBodyData)
{
position = rigidBodyData.Position;
var r = rigidBodyData.Rotation;
init_matrix = Matrix.RotationYawPitchRoll(r.Y, r.X, r.Z) * Matrix.Translation(position);
offset_matrix = Matrix.Invert(init_matrix);
boneIndex = rigidBodyData.BoneIndex;
physicsCalcType = rigidBodyData.PhysicsCalcType;
shape = rigidBodyData.Shape;
}
// 球の剛体を作る
private void CreateSphereRigid(MMDModel model, Vector3 rigid_position)
{
RigidBodyData rigidBody = model.Model.RigidBodyList.RigidBodies[0];
float radius = rigidBody.Size.X;
float mass = rigidBody.Mass;
float restitution = 1; //rigidBody.Repulsion;
float friction = 0; //rigidBody.Friction;
float linear_damp = 0; // rigidBody.MoveAttenuation;
float angular_damp = rigidBody.RotationAttenuation;
Matrix world = Matrix.Translation(rigid_position);
ConvexShape shape;
balls.Add(bulletPhysics.CreateSphere(radius, world, mass, restitution, friction, linear_damp, angular_damp));
count++;
}
internal bool Find(Guid key, out RigidBodyData rigidBodyData)
{
while (_iteratorIndex < _rigidBodies.Count && _rigidBodies.Keys[_iteratorIndex].CompareTo(key) < 0)
{
_iteratorIndex++;
}
if (_iteratorIndex < _rigidBodies.Count &&
_rigidBodies.Keys[_iteratorIndex] == key &&
_rigidBodies.Values[_iteratorIndex].Time >= 0)
{
rigidBodyData = _rigidBodies.Values[_iteratorIndex++];
return(true);
}
else
{
rigidBodyData = null;
return(false);
}
}
static void processFrameData(NatNetML.FrameOfMocapData data)
{
//mPoints.Clear();
//// Standard marker streaming.
//for (int i = 0; i < data.nOtherMarkers; i++)
//{
// // Recreate point data.
// Point3d markerPoint = new Point3d(Math.Round(data.LabeledMarkers[i].x, 4), Math.Round(data.LabeledMarkers[i].y, 4), Math.Round(data.LabeledMarkers[i].z, 4));
// mPoints.Add(markerPoint);
// /*
// for (int j = 0; j < data.MarkerSets[i].nMarkers; j++)
// {
// // Recreate point data.
// Point3d markerPoint = new Point3d(Math.Round(data.LabeledMarkers[i].x, 4), Math.Round(data.LabeledMarkers[i].y, 4), Math.Round(data.LabeledMarkers[i].z, 4));
// mPoints.Add(markerPoint);
// }
// */
// /*
// // Create marker labels.
// string markerLabel = data.OtherMarkers[i].ID.ToString();
// mLabels.Add(markerLabel);
// */
//}
if (RigidBody)
{
rigidBodyNames.Clear();
rigidBodyPos.Clear();
rigidBodyQuat.Clear();
rigidBodyPlanes.Clear();
/* Parsing Rigid Body Frame Data */
for (int i = 0; i < mRigidBodies.Count; i++)
{
// int rbID = mRigidBodies[i].ID; // Fetching rigid body IDs from the saved descriptions
for (int j = 0; j < data.nRigidBodies; j++)
{
RigidBody rb = mRigidBodies[i]; // Saved rigid body descriptions
RigidBodyData rbData = data.RigidBodies[j]; // Received rigid body descriptions
if (rbData.Tracked == true)
{
// Rigid Body ID
rigidBodyNames.Add(rb.Name);
// Rigid Body Position
rigidBodyPos.Add(Math.Round(rbData.x, 4));
rigidBodyPos.Add(Math.Round(rbData.y, 4));
rigidBodyPos.Add(Math.Round(rbData.z, 4));
// Rigid Body Euler Orientation
//float[] quat = new float[4] { rbData.qx, rbData.qy, rbData.qz, rbData.qw };
// Attempt to normalise the rotation notation to fit with robot programming (WXYZ).
rigidBodyQuat.Add(Math.Round(rbData.qw, 6));
rigidBodyQuat.Add(Math.Round(rbData.qx, 6));
rigidBodyQuat.Add(Math.Round(rbData.qy, 6));
rigidBodyQuat.Add(Math.Round(rbData.qz, 6));
/*
* float[] eulers = new float[3];
*
* eulers = NatNetClientML.QuatToEuler(quat, NATEulerOrder.NAT_XYZr); //Converting quat orientation into XYZ Euler representation.
* double xrot = RadiansToDegrees(eulers[0]);
* double yrot = RadiansToDegrees(eulers[1]);
* double zrot = RadiansToDegrees(eulers[2]);
*
* Console.WriteLine("\t\tori ({0:N3}, {1:N3}, {2:N3})", xrot, yrot, zrot);
*/
// Create Plane
Plane rbPlane = new Plane();
Quaternion rbQuat = new Quaternion(Math.Round(rbData.qw, 6), Math.Round(rbData.qx, 6), Math.Round(rbData.qy, 6), Math.Round(rbData.qz, 6));
rbQuat.GetRotation(out rbPlane);
rbPlane.Origin = new Point3d(Math.Round(rbData.x, 4), Math.Round(rbData.y, 4), Math.Round(rbData.z, 4));
//Scale from meter to milli meter
rbPlane.Transform(Transform.Scale(new Point3d(0, 0, 0), 1000));
Transform xformYup = new Transform();
xformYup.M01 = xformYup.M02 = xformYup.M03 = xformYup.M10 = xformYup.M11 = xformYup.M13 = xformYup.M20 = xformYup.M22 = xformYup.M33 = xformYup.M30 = xformYup.M31 = xformYup.M32 = 0;
xformYup.M00 = xformYup.M21 = xformYup.M33 = 1;
xformYup.M12 = -1;
Transform xRotate = new Transform();
xRotate.M00 = xRotate.M02 = xRotate.M03 = xRotate.M11 = xRotate.M12 = xRotate.M13 = xRotate.M20 = xRotate.M21 = xRotate.M33 = xRotate.M30 = xRotate.M31 = xRotate.M32 = 0;
xRotate.M10 = xRotate.M22 = xRotate.M33 = 1;
xRotate.M01 = -1;
if (yUp) //Detect if Y is pointing up
{
rbPlane.Transform(xformYup);
}
rbPlane.Transform(xRotate);
rigidBodyPlanes.Add(rbPlane);
}
else
{
log.Add(rb.Name.ToString() + " is not tracked in current frame.");
}
}
}
}
//if (Skeleton)
//{
// /* Parsing Skeleton Frame Data */
// for (int i = 0; i < mSkeletons.Count; i++) // Fetching skeleton IDs from the saved descriptions
// {
// int sklID = mSkeletons[i].ID;
// for (int j = 0; j < data.nSkeletons; j++)
// {
// if (sklID == data.Skeletons[j].ID) // When skeleton ID of the description matches skeleton ID of the frame data.
// {
// NatNetML.Skeleton skl = mSkeletons[i]; // Saved skeleton descriptions
// NatNetML.SkeletonData sklData = data.Skeletons[j]; // Received skeleton frame data
// Console.WriteLine("\tSkeleton ({0}):", skl.Name);
// Console.WriteLine("\t\tSegment count: {0}", sklData.nRigidBodies);
// /* Now, for each of the skeleton segments */
// for (int k = 0; k < sklData.nRigidBodies; k++)
// {
// NatNetML.RigidBodyData boneData = sklData.RigidBodies[k];
// /* Decoding skeleton bone ID */
// int skeletonID = HighWord(boneData.ID);
// int rigidBodyID = LowWord(boneData.ID);
// int uniqueID = skeletonID * 1000 + rigidBodyID;
// int key = uniqueID.GetHashCode();
// NatNetML.RigidBody bone = (RigidBody)mHtSkelRBs[key]; //Fetching saved skeleton bone descriptions
// //Outputting only the hip segment data for the purpose of this sample.
// if (k == 0)
// Console.WriteLine("\t\t{0:N3}: pos({1:N3}, {2:N3}, {3:N3})", bone.Name, boneData.x, boneData.y, boneData.z);
// }
// }
// }
// }
//}
//if (ForcePlate)
//{
// /* Parsing Force Plate Frame Data */
// for (int i = 0; i < mForcePlates.Count; i++)
// {
// int fpID = mForcePlates[i].ID; // Fetching force plate IDs from the saved descriptions
// for (int j = 0; j < data.nForcePlates; j++)
// {
// if (fpID == data.ForcePlates[j].ID) // When force plate ID of the descriptions matches force plate ID of the frame data.
// {
// NatNetML.ForcePlate fp = mForcePlates[i]; // Saved force plate descriptions
// NatNetML.ForcePlateData fpData = data.ForcePlates[i]; // Received forceplate frame data
// Console.WriteLine("\tForce Plate ({0}):", fp.Serial);
// // Here we will be printing out only the first force plate "subsample" (index 0) that was collected with the mocap frame.
// for (int k = 0; k < fpData.nChannels; k++)
// {
// Console.WriteLine("\t\tChannel {0}: {1}", fp.ChannelNames[k], fpData.ChannelData[k].Values[0]);
// }
// }
// }
// }
//}
}
