void Scale2VR()
{
cameraPosition = vrCamera.transform.position;
float scaleBy = cameraPosition.y / head.position.y;
this.transform.parent.localScale = new Vector3(this.transform.parent.localScale.x * scaleBy, this.transform.parent.localScale.y * scaleBy, this.transform.parent.localScale.z * scaleBy);
//this.transform.parent.position = new Vector3(cameraPosition.x - head.position.x, cameraPosition.y / 2 - head.position.y, cameraPosition.z - head.position.x);
//this.transform.parent.rot
//Get the latest Tranform positions
skeletonPoints[0] = head.position;
skeletonPoints[1] = rightHand.position;
skeletonPoints[2] = leftHand.position;
vrPoints[0] = new Vector4(vrCamera.transform.position.x, vrCamera.transform.position.y, vrCamera.transform.position.z, vrCamera.transform.localScale.x);
vrPoints[1] = new Vector4(vrRightHand.transform.position.x, vrRightHand.transform.position.y, vrRightHand.transform.position.z, vrRightHand.transform.localScale.x);
vrPoints[2] = new Vector4(vrLeftHand.transform.position.x, vrLeftHand.transform.position.y, vrLeftHand.transform.position.z, vrLeftHand.transform.localScale.x);
Matrix4x4 kabschTransform = solver.SolveKabsch(skeletonPoints, vrPoints);
head.position = kabschTransform.MultiplyPoint3x4(skeletonPoints[0]);
rightHand.position = kabschTransform.MultiplyPoint3x4(skeletonPoints[1]);
leftHand.position = kabschTransform.MultiplyPoint3x4(skeletonPoints[2]);
scalingComplete = true;
}
//This function is called when align buttion is clicked.
public void AlignModel()
{
Debug.Log("Align Model");
benchtopSharing.IsManipulated = true;
virtualLandmarkPositions = GetAllLandmarkPositions(virtualLandmarks);
for (int i = 0; i < virtualLandmarks.Length; i++)
{
refPoints[i] = new Vector4(realLandmarkPositions[i].x, realLandmarkPositions[i].y, realLandmarkPositions[i].z, 1);
Debug.Log(refPoints[i]);
}
Matrix4x4 kabschTranform = solver.SolveKabsch(virtualLandmarkPositions, refPoints);
Vector3 beforeCentroid = AlignmentHelper.GetCentroidPosition(virtualLandmarkPositions);
Debug.Log("Before Centroid" + beforeCentroid);
for (int i = 0; i < virtualLandmarks.Length; i++)
{
virtualLandmarkPositions[i] = kabschTranform.MultiplyPoint3x4(virtualLandmarkPositions[i]);
}
Vector3 afterCentroid = AlignmentHelper.GetCentroidPosition(virtualLandmarkPositions);
Debug.Log("After Centroid" + beforeCentroid);
// translate model
model.position += afterCentroid - beforeCentroid;
// rotate model
Quaternion rotation = kabschTranform.GetQuaternion();
model.RotateAroundPivot(afterCentroid, rotation);
}
void Update()
{
solver = new KabschSolver();
for (int iteration = 0; iteration < 2; iteration++)
{
float currentCost = 0f;
for (int i = 0; i < features.Length; i++)
{
refPoints[i] = features[i].position;
inPoints[i] = Constraints.ConstrainToSegment(features[i].position, aligningCamera.position, aligningCamera.TransformPoint(rayDirections[i] * 6f));
currentCost += Vector3.Distance(refPoints[i], inPoints[i]);
Debug.DrawLine(aligningCamera.position, aligningCamera.TransformPoint(rayDirections[i]));
Debug.DrawLine(features[i].position, inPoints[i], Color.red);
}
Matrix4x4 iterationStep = solver.SolveKabsch(inPoints, refPoints, true);
Matrix4x4 steppedTransform = iterationStep * aligningCamera.localToWorldMatrix;
if (useLinearEstimate)
{
float stepScale = ((previousCost - currentCost) > 0.0000001f && iteration > 0) ? (previousCost / (previousCost - currentCost)) * 0.8f : 1f;
aligningCamera.position += (steppedTransform.GetVector3() - aligningCamera.position) * stepScale;
aligningCamera.rotation = Quaternion.SlerpUnclamped(Quaternion.identity, iterationStep.GetQuaternion(), stepScale) * aligningCamera.rotation;
}
else
{
aligningCamera.position = steppedTransform.GetVector3();
aligningCamera.rotation = steppedTransform.GetQuaternion();
}
previousCost = currentCost;
}
}
void Update()
{
for (int iteration = 0; iteration < 1; iteration++)
{
for (int i = 0; i < features.Length; i++)
{
Debug.DrawLine(cameras[0].position, cameras[0].TransformPoint(rayDirections[0][i]));
Debug.DrawLine(cameras[1].position, cameras[1].TransformPoint(rayDirections[1][i]));
float timeLineOne, timeLineTwo;
line2lineDisplacement(cameras[0].position, cameras[0].TransformPoint(rayDirections[0][i]), cameras[1].position, cameras[1].TransformPoint(rayDirections[1][i]), out timeLineOne, out timeLineTwo);
//Take the abs of the times so they can't intersect behind the camera
timeLineOne = Mathf.Abs(timeLineOne); timeLineTwo = Mathf.Abs(timeLineTwo);
inPoints[i] = Vector3.LerpUnclamped(cameras[0].position, cameras[0].TransformPoint(rayDirections[0][i]), timeLineOne);
refPoints[i] = Vector3.LerpUnclamped(cameras[1].position, cameras[1].TransformPoint(rayDirections[1][i]), timeLineTwo);
Debug.DrawLine(inPoints[i], refPoints[i], Color.red);
}
Matrix4x4 iterationStep = solver.SolveKabsch(refPoints, inPoints, true);
cameras[1].position += iterationStep.GetVector3();
cameras[1].rotation = iterationStep.GetQuaternion() * cameras[1].rotation;
}
}
void Update()
{
for (int cameraIndex = 0; cameraIndex < 4; cameraIndex++)
{
for (int i = 0; i < features.Length; i++)
{
Debug.DrawLine(cameras[cameraIndex].position, cameras[cameraIndex].TransformPoint(rayDirections[cameraIndex][i]));
}
}
Profiler.BeginSample("Bundle Adjustment");
for (int iteration = 0; iteration < 1; iteration++)
{
for (int i = 0; i < features.Length; i++)
{
for (int cameraIndex = 0; cameraIndex < 4; cameraIndex++)
{
Vector3 pointLineOne, pointLineTwo;
Displacement(cameras[(cameraIndex / 2)].position, cameras[(cameraIndex / 2)].TransformPoint(rayDirections[(cameraIndex / 2)][i]),
cameras[(cameraIndex % 2) + 2].position, cameras[(cameraIndex % 2) + 2].TransformPoint(rayDirections[(cameraIndex % 2) + 2][i]), out pointLineOne, out pointLineTwo);
inPoints [(i * cameras.Length) + cameraIndex] = pointLineOne;
refPoints[(i * cameras.Length) + cameraIndex] = pointLineTwo;
Debug.DrawLine(pointLineOne, pointLineTwo, Color.red);
}
}
Matrix4x4 iterationStep = solver.SolveKabsch(refPoints, inPoints, true);
cameras[2].parent.position += iterationStep.GetVector3();
cameras[2].parent.rotation = iterationStep.GetQuaternion() * cameras[2].parent.rotation;
}
Profiler.EndSample();
}
void Update()
{
//Translate the points into world space
for (int i = 0; i < bodyVerts.Length; i++)
{
bodyVerts[i] = transform.TransformPoint(bodyVerts[i]);
}
for (int i = 0; i < solverIterations; i++)
{
//First, ensure that the surface area is what we think it is
Verlet.resolveDistanceConstraints(constraints, ref bodyVerts, ref accumulatedDisplacements, 1);
//Next, set the volume of the soft body
Verlet.setVolume(inflationAmount * initialVolume, bodyVerts, bodyNormals, bodyTriangles, initialSurfaceArea, true, fastButGarbage);
}
//Also clamp to the intersection of capsules for shits
for (int j = 0; j < bodyVerts.Length; j++)
{
Vector3 capOne = Constraints.ConstrainToCapsule(bodyVerts[j], Vector3.zero, Vector3.up, 0.25f) - bodyVerts[j];
Vector3 capTwo = Constraints.ConstrainToCapsule(bodyVerts[j], Vector3.zero, Vector3.right * 1.15f, 0.25f) - bodyVerts[j];
if (capOne.sqrMagnitude < capTwo.sqrMagnitude)
{
bodyVerts[j] += capOne;
}
else
{
bodyVerts[j] += capTwo;
}
}
//Calculate the the position and rotation of the body
for (int i = 0; i < bodyVerts.Length; i++)
{
kabschVerts[i] = new Vector4(bodyVerts[i].x, bodyVerts[i].y, bodyVerts[i].z, 1f);
}
;
Matrix4x4 toWorldSpace = kabschSolver.SolveKabsch(originalVerts, Array.ConvertAll(bodyVerts, (p => (Vector4)p)));
transform.position = toWorldSpace.GetVector3();
transform.rotation = toWorldSpace.GetQuaternion();
//Move the points into local space for rendering
for (int i = 0; i < bodyVerts.Length; i++)
{
bodyVerts[i] = transform.InverseTransformPoint(bodyVerts[i]);
renderNormals[i] = transform.InverseTransformDirection(bodyNormals[i]);
}
Debug.Log(Verlet.VolumeOfMesh(bodyVerts, bodyTriangles));
//Graphics
bodyMesh.vertices = bodyVerts;
bodyMesh.normals = renderNormals;
bodyMesh.RecalculateBounds();
bodyMesh.UploadMeshData(false);
}
public static Quaternion GetRotation(List <Vector3> a, List <Vector3> b)
{
var closest = ComputePointMapping(a, b);
var c = (from i in closest.Values select b[i]).ToList();
KabschSolver solver = new KabschSolver();
var matrix = solver.SolveKabsch(a, c);
return(matrix.GetQuaternion());
}
//Calculate the Kabsch Transform and Apply it to the input points
void Update()
{
for (int i = 0; i < inPoints.Length; i++)
{
refPoints[i] = new Vector4(referencePoints[i].position.x, referencePoints[i].position.y, referencePoints[i].position.z, referencePoints[i].localScale.x);
}
Matrix4x4 kabschTransform = solver.SolveKabsch(points, refPoints);
for (int i = 0; i < inPoints.Length; i++)
{
inPoints[i].position = kabschTransform.MultiplyPoint3x4(points[i]);
}
}
//Calculate the Kabsch Transform and Apply it to the input points
void Update()
{
for (int i = 0; i < inPoints.Length; i++)
{
refPoints[i] = referencePoints[i].position;
}
Matrix4x4 kabschTransform = solver.SolveKabsch(points, refPoints);
for (int i = 0; i < inPoints.Length; i++)
{
inPoints[i].position = kabschTransform.MultiplyPoint3x4(points[i]);
}
}
void Update()
{
//Translate the points into world space
for (int i = 0; i < bodyVerts.Length; i++)
{
bodyVerts[i] = transform.TransformPoint(bodyVerts[i]);
}
//Physics
float currentDeltaTime = Mathf.Clamp(Time.deltaTime, 0.01f, previousDeltaTime * 1.4f);
Verlet.Integrate(bodyVerts, prevBodyVerts, scaledGravity, currentDeltaTime, previousDeltaTime);
previousDeltaTime = currentDeltaTime;
//Anchor a point on the body
if (anchor != null && anchor.gameObject.activeInHierarchy)
{
bodyVerts[0] = prevBodyVerts[0] = anchor.position;
}
//Also sneak in a ground plane here:
Vector3 groundPlanePos = groundPlane.position;
Vector3 groundPlaneNormal = -groundPlane.forward;
for (int j = 0; j < bodyVerts.Length; j++)
{
if (Vector3.Dot(bodyVerts[j] - groundPlanePos, groundPlaneNormal) < 0f)
{
bodyVerts[j] = Vector3.ProjectOnPlane(bodyVerts[j] - groundPlanePos, groundPlaneNormal) + groundPlanePos;
bodyVerts[j] -= Vector3.ProjectOnPlane(bodyVerts[j] - prevBodyVerts[j], groundPlaneNormal) * 0.3f;
}
}
//Calculate the the position and rotation of the body
for (int i = 0; i < bodyVerts.Length; i++)
{
kabschVerts[i] = new Vector4(bodyVerts[i].x, bodyVerts[i].y, bodyVerts[i].z, 1f);
}
;
kabschVerts.CopyTo(kabschVertsArray);
Matrix4x4 toWorldSpace = kabschSolver.SolveKabsch(originalVerts, kabschVertsArray, true);
constrainVertsToDeformation(originalVerts, toWorldSpace, ref bodyVerts);
Verlet.RecalculateNormalsNonAlloc(bodyVerts, bodyTriangles, ref bodyNormals);
//Move the points into local space for rendering
transform.position = toWorldSpace.GetVector3();
transform.rotation = toWorldSpace.GetQuaternion();
for (int i = 0; i < bodyVerts.Length; i++)
{
bodyVerts[i] = transform.InverseTransformPoint(bodyVerts[i]);
renderNormals[i] = transform.InverseTransformDirection(bodyNormals[i]);
}
Debug.DrawRay(transform.position, toWorldSpace * (Vector3.Cross(yBasis, zBasis).normalized *xScale), Color.red);
Debug.DrawRay(transform.position, toWorldSpace * yBasis, Color.green);
Debug.DrawRay(transform.position, toWorldSpace * zBasis, Color.blue);
//Graphics
bodyVerts.CopyTo(bodyVertsArray);
renderNormals.CopyTo(renderNormalsArray);
bodyMesh.vertices = bodyVertsArray;
bodyMesh.normals = renderNormalsArray;
bodyMesh.RecalculateBounds();
bodyMesh.UploadMeshData(false);
}
void Update()
{
//Translate the points into world space
for (int i = 0; i < bodyVerts.Length; i++)
{
bodyVerts[i] = transform.TransformPoint(bodyVerts[i]);
}
//Physics
float currentDeltaTime = Mathf.Clamp(Time.deltaTime, 0.01f, previousDeltaTime * 1.4f);
Verlet.Integrate(bodyVerts, prevBodyVerts, scaledGravity, currentDeltaTime, previousDeltaTime);
previousDeltaTime = currentDeltaTime;
//Anchor a point on the body
if (anchor != null && anchor.gameObject.activeInHierarchy)
{
bodyVerts[0] = prevBodyVerts[0] = anchor.position;
}
for (int i = 0; i < solverIterations; i++)
{
//First, ensure that the surface area is what we think it is
Verlet.resolveDistanceConstraints(constraints, ref bodyVerts, ref accumulatedDisplacements, 1);
//Next, set the volume of the soft body
Verlet.setVolume(inflationAmount * initialVolume, bodyVerts, bodyNormals, bodyTriangles, initialSurfaceArea, true, fastButGarbage);
}
//Also sneak in a ground plane here:
Vector3 groundPlanePos = groundPlane.position;
Vector3 groundPlaneNormal = -groundPlane.forward;
for (int j = 0; j < bodyVerts.Length; j++)
{
if (Vector3.Dot(bodyVerts[j] - groundPlanePos, groundPlaneNormal) < 0f)
{
bodyVerts[j] = Vector3.ProjectOnPlane(bodyVerts[j] - groundPlanePos, groundPlaneNormal) + groundPlanePos;
bodyVerts[j] -= Vector3.ProjectOnPlane(bodyVerts[j] - prevBodyVerts[j], groundPlaneNormal) * 0.3f;
}
}
//Calculate the the position and rotation of the body
for (int i = 0; i < bodyVerts.Length; i++)
{
kabschVerts[i] = new Vector4(bodyVerts[i].x, bodyVerts[i].y, bodyVerts[i].z, 1f);
}
;
Matrix4x4 toWorldSpace = kabschSolver.SolveKabsch(originalVerts, kabschVerts, transformFollowsRotation);
transform.position = toWorldSpace.GetVector3();
transform.rotation = toWorldSpace.GetQuaternion();
//Move the points into local space for rendering
for (int i = 0; i < bodyVerts.Length; i++)
{
bodyVerts[i] = transform.InverseTransformPoint(bodyVerts[i]);
renderNormals[i] = transform.InverseTransformDirection(bodyNormals[i]);
}
//Graphics
bodyMesh.vertices = bodyVerts;
bodyMesh.normals = renderNormals;
bodyMesh.RecalculateBounds();
bodyMesh.UploadMeshData(false);
}
private void Update()
{
//Update the kinect data
for (int i = 0; i < users.Length; i++)
{
if (users[i].skeletonProvider != null)
{
KinectSkeleton skeleton = users[i].GetSkeleton();
if (skeleton != null && skeleton.valid)
{
if (!users[i].created)
{
users[i].skeletonProvider.CreateSkeleton(users[i]);
users[i].created = true;
}
if (users[i].updateFromKinect)
{
/*transform.position = getHeadPosition();
* if (lookAt != null)
* {
* transform.LookAt(lookAt.transform.position);
* }
* //else transform.localRotation = Quaternion.identity;*/
users[i].skeletonProvider.RefreshBodyObject(users[i]);
}
}
}
}
#region Kabsch implementation
if (kabsch)
{
//Get the latest Tranform positions
for (int i = 0; i < users.Length; i++)
{
if (users[i].GetSkeleton() != null && users[i].GetSkeleton().valid)
{
for (int j = 0; j < users[i].skeletonProvider.joints.Length; j++)
{
pointSets[i][j] = users[i].skeletonProvider.joints[j].position;
}
}
}
for (int j = 0; j < targetPointSet.Length; j++)
{
if (users[0].GetSkeleton() != null && users[0].GetSkeleton().valid)
{
targetPointSet[j] = new Vector4(users[0].skeletonProvider.joints[j].position.x, users[0].skeletonProvider.joints[j].position.y, users[0].skeletonProvider.joints[j].position.z, users[0].skeletonProvider.joints[j].localScale.x);
}
}
//Calculate new positions for the skeletons
for (int k = 1; k < users.Length; k++)
{
if (users[k].GetSkeleton() != null && users[k].GetSkeleton().valid)
{
Matrix4x4 kabschTransform = solver.SolveKabsch(pointSets[k], targetPointSet);
for (int i = 0; i < users[k].skeletonProvider.joints.Length; i++)
{
users[k].skeletonProvider.joints[i].position = kabschTransform.MultiplyPoint3x4(pointSets[k][i]);
for (int j = 0; j < HumanTopology.BONE_CONNECTIONS.Length; j++)
{
BoneConnection bone = HumanTopology.BONE_CONNECTIONS[j];
JointType joint1 = bone.fromJoint;
JointType joint2 = bone.toJoint;
LineRenderer lr = users[k].skeletonProvider.joints[(int)joint2].GetComponent <LineRenderer>();
lr.SetPosition(0, users[k].skeletonProvider.joints[(int)joint1].position);
lr.SetPosition(1, users[k].skeletonProvider.joints[(int)joint2].position);
}
/*for (int j = 0; j < HumanTopology.BONE_CONNECTIONS.Length; j++)
* {
* BoneConnection bone = HumanTopology.BONE_CONNECTIONS[j];
* JointType joint1 = bone.fromJoint;
* JointType joint2 = bone.toJoint;
* KinectSkeleton.TrackingState state1 = users[k].GetSkeleton().jointStates[(int)joint1];
* KinectSkeleton.TrackingState state2 = users[k].GetSkeleton().jointStates[(int)joint2];
* LineRenderer lr = users[k].skeletonProvider.joints[i].GetComponent<LineRenderer>();
* bool tracked = state1 != KinectSkeleton.TrackingState.NotTracked && state2 != KinectSkeleton.TrackingState.NotTracked;
* if (tracked)
* {
* lr.SetPosition(0, users[k].skeletonProvider.joints[(int)joint1].position);
* lr.SetPosition(1, users[k].skeletonProvider.joints[(int)joint2].position);
* }
* }*/
}
}
}
}
#endregion
#region Kalman implementation
if (kalman)
{
for (int i = 0; i < HumanTopology.JOINT_NAMES.Length; i++)
{
for (int j = 0; j < users.Length; j++)
{
if (users[j].GetSkeleton() != null && users[j].GetSkeleton().valid)
{
filters[i].PredictState(Time.deltaTime);
filters[i].UpdateState(new double[, ]
{
{ users[j].skeletonProvider.joints[i].position.x },
{ users[j].skeletonProvider.joints[i].position.y },
{ users[j].skeletonProvider.joints[i].position.z },
{ 0.0 },
{ 0.0 },
{ 0.0 },
{ 0.0 },
{ 0.0 },
{ 0.0 }
});
double oldPos = filters[i].StateMatrix[0, 0];
/*for (int k = 1; k < 10; k++)
* {
* double[,] PredictedState = filters[i].SafePredictState(0.1 * k);
* Debug.DrawLine(new Vector3((float)oldPos, k * 0.02f, 0f), new Vector3((float)PredictedState[0, 0], (k + 1) * 0.02f, 0f), Color.green);
* oldPos = PredictedState[0, 0];
*
* Debug.DrawLine(new Vector3(Mathf.Sin((Time.time + (k * 0.1f))), k * 0.02f, 0f),
* new Vector3(Mathf.Sin((Time.time + ((k + 1f) * 0.1f))), (k + 1) * 0.02f, 0f), Color.red);
* }*/
fusedJoints[i].position = new Vector3((float)filters[i].StateMatrix[0, 0], (float)filters[i].StateMatrix[1, 0], (float)filters[i].StateMatrix[2, 0]);
}
}
}
for (int k = 0; k < HumanTopology.BONE_CONNECTIONS.Length; k++)
{
BoneConnection bone = HumanTopology.BONE_CONNECTIONS[k];
JointType joint1 = bone.fromJoint;
JointType joint2 = bone.toJoint;
LineRenderer lr = fusedJoints[(int)joint2].GetComponent <LineRenderer>();
lr.SetPosition(0, fusedJoints[(int)joint1].position);
lr.SetPosition(1, fusedJoints[(int)joint2].position);
}
readyToScale = true;
}
#endregion
}
// Update is called once per frame
void Update()
{
if (devices.Length > 1)
{
// Add the set of joints to device-specific lists
if (Input.GetKeyUp(takeCalibrationSampleKey))
{
bool handInAllFrames = true;
for (int i = 0; i < devices.Length; i++)
{
Hand rightHand = devices[i].deviceProvider.CurrentFrame.Get(Chirality.Right);
if (rightHand != null)
{
devices[i].currentHand = rightHand;
}
else
{
handInAllFrames = false;
}
}
if (handInAllFrames)
{
for (int i = 0; i < devices.Length; i++)
{
if (devices[i].handPoints == null)
{
devices[i].handPoints = new List <Vector3>();
}
for (int j = 0; j < 5; j++)
{
for (int k = 0; k < 4; k++)
{
devices[i].handPoints.Add(devices[i].currentHand.Fingers[j].bones[k].Center.ToVector3());
}
}
}
}
}
// Moves subsidiary devices to be in alignment with the device at the 0 index
if (Input.GetKeyUp(solveForRelativeTransformKey))
{
if (devices[0].handPoints.Count > 3)
{
for (int i = 1; i < devices.Length; i++)
{
KabschSolver solver = new KabschSolver();
Matrix4x4 deviceToOriginDeviceMatrix =
solver.SolveKabsch(devices[i].handPoints, devices[0].handPoints, 200);
devices[i].deviceProvider.transform.Transform(deviceToOriginDeviceMatrix);
devices[i].handPoints.Clear();
}
devices[0].handPoints.Clear();
}
}
}
}
void Update()
{
Profiler.BeginSample("Schedule Softbody Work", this);
//Transform the points into world space
JobHandle localToWorldHandle = new ToWorldSpaceJob()
{
localToWorld = transform.localToWorldMatrix,
bodyVerts = softbodyData.bodyVerts
}.Schedule(originalVerts.Length, 64);
//Physics - Verlet Integration
JobHandle verletHandle = new VerletIntegrateJob()
{
bodyVerts = softbodyData.bodyVerts,
prevBodyVerts = softbodyData.prevBodyVerts,
scaledGravity = softbodyData.scaledGravity
}.Schedule(originalVerts.Length, 64, dependsOn: localToWorldHandle);
JobHandle previousHandle = verletHandle;
for (int i = 0; i < solverIterations; i++)
{
//First, ensure that the surface area is what we think it is
JobHandle accumulateDistances = new AccumulateDistanceConstraintsJob()
{
bodyVerts = softbodyData.bodyVerts,
accumulatedDisplacements = softbodyData.accumulatedDisplacements,
constraintsArray = softbodyData.constraintsArray
}.Schedule(dependsOn: previousHandle);
JobHandle applyConstraints = new ApplyAccumulatedConstraintsJob()
{
bodyVerts = softbodyData.bodyVerts,
accumulatedDisplacements = softbodyData.accumulatedDisplacements
}.Schedule(originalVerts.Length, 64, dependsOn: accumulateDistances);
//Next, set the volume of the soft body
JobHandle accumulateNormals = new AccumulateNormalsJob()
{
bodyVerts = softbodyData.bodyVerts,
bodyTriangles = softbodyData.bodyTriangles,
bodyNormals = softbodyData.bodyNormals
}.Schedule(dependsOn: applyConstraints);
JobHandle normalizeNormals = new NormalizeNormalsJob()
{
bodyNormals = softbodyData.bodyNormals
}.Schedule(originalVerts.Length, 64, dependsOn: accumulateNormals);
JobHandle calculateDilationDistance = new CalculateSurfaceAreaAndVolumeJob()
{
bodyVerts = softbodyData.bodyVerts,
bodyTriangles = softbodyData.bodyTriangles,
initialVolume = softbodyData.initialVolume,
initialSurfaceArea = softbodyData.initialSurfaceArea,
dilationDistance = softbodyData.dilationDistance
}.Schedule(dependsOn: normalizeNormals);
previousHandle = new ExtrudeNormalsJob()
{
bodyVerts = softbodyData.bodyVerts,
bodyNormals = softbodyData.bodyNormals,
dilationDistance = softbodyData.dilationDistance
}.Schedule(originalVerts.Length, 64, dependsOn: calculateDilationDistance);
}
//Also sneak in a ground plane here:
JobHandle groundPlaneHandle = new GroundCollideJob()
{
bodyVerts = softbodyData.bodyVerts,
prevBodyVerts = softbodyData.prevBodyVerts,
groundPlanePos = groundPlane.position,
groundPlaneNormal = -groundPlane.forward
}.Schedule(originalVerts.Length, 64, dependsOn: previousHandle);
Profiler.EndSample();
groundPlaneHandle.Complete();
//Calculate the the position and rotation of the body
for (int i = 0; i < softbodyData.bodyVerts.Length; i++)
{
softbodyData.kabschVerts[i] = new Vector4(
softbodyData.bodyVerts[i].x,
softbodyData.bodyVerts[i].y,
softbodyData.bodyVerts[i].z, 1f);
}
;
softbodyData.kabschVerts.CopyTo(kabschVertsArray);
Matrix4x4 toWorldSpace = kabschSolver.SolveKabsch(originalVerts, kabschVertsArray, transformFollowsRotation);
transform.position = toWorldSpace.GetVector3();
transform.rotation = toWorldSpace.GetQuaternion();
//Move the points into local space for rendering
JobHandle toLocalHandle = new ToLocalSpaceJob()
{
bodyVerts = softbodyData.bodyVerts,
bodyNormals = softbodyData.bodyNormals,
renderNormals = softbodyData.renderNormals,
worldToLocal = transform.worldToLocalMatrix
}.Schedule(originalVerts.Length, 64);
toLocalHandle.Complete();
//Graphics
softbodyData.bodyVerts.CopyTo(bodyVertsArray);
softbodyData.renderNormals.CopyTo(renderNormalsArray);
bodyMesh.vertices = bodyVertsArray;
bodyMesh.normals = renderNormalsArray;
bodyMesh.RecalculateBounds();
bodyMesh.UploadMeshData(false);
}
IEnumerator CalibrateScreenLocation()
{
//Reset some stuff
HyperMegaStuff.HyperMegaLines.drawer.dontClear = false;
monitorPattern.SetActive(false);
headsetPattern.SetActive(false);
for (int i = 0; i < calibrationDevices.Length; i++)
{
if (calibrationDevices[i].isConnected)
{
calibrationDevices[i].resetMasks();
}
calibrationDevices[i].undistortImage = false; // Stop undistorting the image because it's slow
}
whiteCircle.gameObject.SetActive(false);
//1) Take a background subtraction shot of a black screen
yield return(new WaitForSeconds(standardDelay));
float startTime = Time.unscaledTime;
while (Time.unscaledTime < startTime + standardDelay * 10f)
{
//Take the max of the current image against the current subtraction image
updateSubtractionBackgrounds();
yield return(null);
}
yield return(new WaitForSeconds(standardDelay));
//2) Unhide a white object
monitorWhiteness.SetActive(true);
yield return(new WaitForSeconds(standardDelay));
//3) Hit "Create Mask"
// Create the binary masks from the subtracted images
createBinaryMasks(monitorMaskThreshold);
yield return(new WaitForSeconds(standardDelay));
monitorWhiteness.SetActive(false);
//4) Start Moving the Circle Around
whiteCircle.SetActive(true);
yield return(new WaitForSeconds(standardDelay / 2f));
HyperMegaStuff.HyperMegaLines drawer = HyperMegaStuff.HyperMegaLines.drawer;
drawer.dontClear = true;
for (int i = 0; i < _realDots.Count; i++)
{
whiteCircle.transform.position = calibrationDotsParent.GetChild(i).position;
int foundDots = 0;
while (foundDots == 0)
{
yield return(new WaitForSeconds(standardDelay));
foundDots = 0;
for (int j = 0; j < calibrationDevices.Length; j++)
{
Vector3 triangulatedDot = triangulate(j, drawer);
if (triangulatedDot != Vector3.zero)
{
calibrationDevices[j].triangulatedDots[i] = triangulatedDot;
foundDots++;
}
}
}
}
whiteCircle.SetActive(false);
drawer.dontClear = false;
//Kabsch the Dots, move the screen!
KabschSolver solver = new KabschSolver();
//Place the monitor based off of one of the webcams
List <Vector3> _triangulatedDots = new List <Vector3>(), _monitorDots = new List <Vector3>();
for (int j = 0; j < _realDots.Count; j++)
{
if (calibrationDevices[0].triangulatedDots[j] != Vector3.zero)
{
_monitorDots.Add(calibrationDotsParent.GetChild(j).position);
_triangulatedDots.Add(calibrationDevices[0].triangulatedDots[j]);
}
if (calibrationDevices[1].triangulatedDots[j] != Vector3.zero)
{
_monitorDots.Add(calibrationDotsParent.GetChild(j).position);
_triangulatedDots.Add(calibrationDevices[1].triangulatedDots[j]);
}
}
CalibrationMonitor.Transform(solver.SolveKabsch(_monitorDots, _triangulatedDots, 200));
for (int j = 0; j < _realDots.Count; j++)
{
_realDots[j] = calibrationDotsParent.GetChild(j).position;
}
monitorPattern.SetActive(true);
headsetPattern.SetActive(true);
}
