private void RotateVector(ref Vector3 startVector, ref Vector2 xyRotateVector, out Vector3 resultLookVector)
{
var startCFrame = new CFrame(Vector3.Zero, startVector);
resultLookVector =
(CFrame.Angles(0, -xyRotateVector.x, 0) * startCFrame * CFrame.Angles(-xyRotateVector.y, 0, 0))
.lookVector;
}
public void UnitVectors()
{
var actualCF = new CFrame(1, 2, 3) * CFrame.Angles(4, 5, 6);
var expectedForward = new Vector3(0.958924294f, -0.214676261f, 0.185413986f);
Assert.AreEqual(actualCF.forward, expectedForward, "forward did not return expected vector.");
Assert.AreEqual(actualCF.lookVector, expectedForward, "lookVector did not return expected vector.");
// TODO: get expected left/right/top/bottom/front/back vectors from roblox
}
public void Lerp()
{
var expected = new CFrame(2.5f, 3.5f, 4.5f, 0.858355939f, -0.272098392f, 0.434956968f, 0.359638363f,
0.923728108f, -0.131858498f, -0.365903497f, 0.269608736f, 0.890744507f);
var p1 = new CFrame(1, 2, 3) * CFrame.Angles(0.1f, 0.2f, 0.3f);
var p2 = new CFrame(4, 5, 6) * CFrame.Angles(0.2f, 0.7f, 0.3f);
var actual = p1.lerp(p2, 0.5f);
Assert.AreEqual(expected, actual, "Rotation * Translation did not return expected results.");
}
public void Composition()
{
var expected1 = new CFrame(1, 2, 3, 0.936293423f, -0.289629489f, 0.198669329f, 0.312991828f, 0.944702566f,
-0.0978434011f, -0.159345075f, 0.153792009f, 0.975170374f);
var actual1 = new CFrame(1, 2, 3) * CFrame.Angles(0.1f, 0.2f, 0.3f);
Assert.AreEqual(expected1, actual1, "Translation * Rotation did not return expected results.");
var expected2 = new CFrame(0.953042448f, 1.90886664f, 3.07375002f, 0.936293423f, -0.289629489f, 0.198669329f,
0.312991828f, 0.944702566f, -0.0978434011f, -0.159345075f, 0.153792009f, 0.975170374f);
var actual2 = CFrame.Angles(0.1f, 0.2f, 0.3f) * new CFrame(1, 2, 3);
Assert.AreEqual(expected2, actual2, "Rotation * Translation did not return expected results.");
}
public static void PatchAngles(ref CFrame applyTo, int axis, float[] angles)
{
float halfPi = (float)Math.PI / 2f;
float[] applyRepair = new float[3];
applyRepair[axis] = halfPi;
float[] applyRotate = new float[3];
applyRotate[axis] = angles[axis];
CFrame repair = CFrame.Angles(applyRepair);
CFrame rotate = CFrame.Angles(applyRotate);
applyTo *= repair * rotate * repair.Inverse();
}
public void EulerAngles()
{
var expectedMatrix = new[]
{
0f, 0f, 0f, 0.936293423f, -0.289629489f, 0.198669329f, 0.312991828f, 0.944702566f, -0.0978434011f,
-0.159345075f,
0.153792009f, 0.975170374f
};
var expectedEuler = new[] { 0.1f, 0.2f, 0.3f };
var cframe = CFrame.Angles(0.1f, 0.2f, 0.3f);
var resultEuler = cframe.getEulerAngles().ToArray();
AssertArraysEqual(cframe.GetComponents(), expectedMatrix, 12,
"CFrame.Angles() matrix did not return expected values.");
AssertArraysEqual(expectedEuler, resultEuler, 3, "CFrame.getEulerAngles() did not return expected values.");
}
public static void PatchAngles(ref CFrame applyTo, int axis, float[] angles)
{
Contract.Requires(angles != null);
const float halfPi = (float)(Math.PI / 2f);
float[] applyRepair = new float[3];
applyRepair[axis] = halfPi;
float[] applyRotate = new float[3];
applyRotate[axis] = angles[axis];
CFrame repair = CFrame.Angles(applyRepair);
CFrame rotate = CFrame.Angles(applyRotate);
applyTo *= repair * rotate * repair.Inverse();
}
public void OnHandleSet(IntPtr handle)
{
_canvas = new CanvasWin32(handle)
{
Parent = Game.CoreEnvironment,
CurrentCamera =
{
CFrame = new CFrame(0, 0, -10) * CFrame.Angles(0, Mathf.Pi, 0),
CameraSubject = _character,
CameraType = CameraType.Custom
}
};
_viewportGui.Parent = _canvas.CurrentCamera;
_previewPart = new Part {
Parent = _canvas, Size = new Vector3(5, 5, 5)
};
Preview = PreviewShape.Sphere;
}
public void Spatial()
{
var par1 = new CFrame(1, 2, 3) * CFrame.Angles(4, 5, 6);
var par2 = new CFrame(4, 5, 6) * CFrame.Angles(1, 2, 3);
var expected1 = new CFrame(2.284338f, -3.74210596f, -2.78898597f, -0.723131239f, -0.25124836f, 0.643393695f,
-0.223633111f, 0.966485798f, 0.126068801f, -0.653505504f, -0.0527198762f, -0.755083561f);
var actual1 = par1.toObjectSpace(par2);
Assert.AreEqual(expected1, actual1, "toObjectSpace did not return expected results.");
var expected2 = new CFrame(-3.26779175f, 4.68169117f, -4.18292999f, -0.522057056f, 0.697389245f,
0.491024077f, 0.781638145f, 0.160808325f, 0.602646112f, 0.341318101f, 0.698418856f, -0.629057229f);
var actual2 = par1.toWorldSpace(par2);
Assert.AreEqual(expected2, actual2, "toWorldSpace did not return expected results.");
var expected3 = new Vector3(2.284338f, -3.74210548f, -2.78898621f);
var actual3 = par1.pointToObjectSpace(par2.p);
Assert.AreEqual(expected3, actual3, "pointToObjectSpace did not return expected results.");
var expected4 = new Vector3(-3.26779175f, 4.68169117f, -4.18292999f);
var actual4 = par1.pointToWorldSpace(par2.p);
Assert.AreEqual(expected4, actual4, "pointToWorldSpace did not return expected results.");
var expected5 = new Vector3(3.14449883f, -7.21789789f, -3.87479973f);
var actual5 = par1.vectorToObjectSpace(par2.p);
Assert.AreEqual(expected5, actual5, "vectorToObjectSpace did not return expected results.");
var expected6 = new Vector3(-4.26779175f, 2.68169117f, -7.18292999f);
var actual6 = par1.vectorToWorldSpace(par2.p);
Assert.AreEqual(expected6, actual6, "vectorToWorldSpace did not return expected results.");
}
public void Update(ref CFrame adorneeCF, ref Vector3 adorneeSize, ref float[] scales)
{
var id = (int)_normalId;
var scale = scales[id];
var cylinderThickness = _cylinderThickness * scale;
var cylinderLength = _cylinderLength * scale;
var coneThickness = _coneThickness * scale;
var coneLength = _coneLength * scale;
CFrame angles;
switch (_normalId)
{
case NormalId.Top:
angles = CFrame.Angles(0, 0, 0);
break;
case NormalId.Bottom:
angles = CFrame.Angles(Mathf.Pi, 0, 0);
break;
case NormalId.Right:
angles = CFrame.Angles(0, 0, -Mathf.Pi / 2);
break;
case NormalId.Left:
angles = CFrame.Angles(0, 0, Mathf.Pi / 2);
break;
case NormalId.Back:
angles = CFrame.Angles(Mathf.Pi / 2, 0, 0);
break;
case NormalId.Front:
angles = CFrame.Angles(-Mathf.Pi / 2, 0, 0);
break;
default:
throw new ArgumentOutOfRangeException();
}
switch (_shape)
{
case Shape.Sphere:
break;
case (Shape)5:
var cylinderCF = _handle._cylinders[id].CFrame;
Size = new Vector3(coneThickness, coneLength, coneThickness);
CFrame = cylinderCF + cylinderCF.up * (cylinderLength / 2 + coneLength / 2);
break;
case Shape.Cylinder:
Size = new Vector3(cylinderThickness, cylinderLength, cylinderThickness);
CFrame = (adorneeCF +
adorneeCF[_normalId] * (ComponentValueFromNormalId(ref adorneeSize, ref _normalId) / 2 + cylinderLength / 2)) *
angles;
break;
default:
throw new ArgumentOutOfRangeException();
}
UpdateRenderData();
}
public static string Assemble(KeyframeSequence sequence, List <Bone> rig)
{
StudioMdlWriter animWriter = new StudioMdlWriter();
List <Keyframe> keyframes = new List <Keyframe>();
Dictionary <string, Bone> boneLookup = new Dictionary <string, Bone>();
List <Node> nodes = animWriter.Nodes;
foreach (Bone bone in rig)
{
Node node = bone.Node;
if (node != null)
{
nodes.Add(node);
string boneName = node.Name;
if (!boneLookup.ContainsKey(boneName))
{
boneLookup.Add(boneName, bone);
}
}
}
foreach (Keyframe kf in sequence.GetChildrenOfClass <Keyframe>())
{
Pose rootPart = kf.FindFirstChild <Pose>("HumanoidRootPart");
if (rootPart != null)
{
// We don't need the rootpart for this.
foreach (Pose subPose in rootPart.GetChildrenOfClass <Pose>())
{
subPose.Parent = kf;
}
rootPart.Destroy();
}
kf.Time /= sequence.TimeScale;
keyframes.Add(kf);
}
keyframes.Sort(0, keyframes.Count, sorter);
Keyframe lastKeyframe = keyframes[keyframes.Count - 1];
float fLength = lastKeyframe.Time;
int frameCount = ToFrameRate(fLength);
// Animations in source are kinda dumb, because theres no frame interpolation.
// I have to account for every single CFrame for every single frame.
Dictionary <int, Dictionary <string, Pose> > keyframeMap = new Dictionary <int, Dictionary <string, Pose> >();
for (int i = 0; i <= frameCount; i++)
{
keyframeMap[i] = new Dictionary <string, Pose>();
}
foreach (Keyframe kf in keyframes)
{
int frame = ToFrameRate(kf.Time);
Dictionary <string, Pose> poseMap = keyframeMap[frame];
List <Pose> poses = GatherPoses(kf);
foreach (Pose pose in poses)
{
poseMap[pose.Name] = pose;
}
}
List <BoneKeyframe> boneKeyframes = animWriter.Skeleton;
Keyframe baseFrame = keyframes[0];
for (int i = 0; i < frameCount; i++)
{
BoneKeyframe frame = new BoneKeyframe();
frame.Time = i;
if (sequence.AvatarType == AvatarType.R15)
{
frame.DeltaSequence = true;
frame.BaseRig = rig;
}
List <Bone> bones = frame.Bones;
foreach (Node node in nodes)
{
PosePair closestPoses = GetClosestPoses(keyframeMap, i, node.Name);
float current = i;
float min = closestPoses.Min.Frame;
float max = closestPoses.Max.Frame;
float alpha = (min == max ? 0 : (current - min) / (max - min));
Pose pose0 = closestPoses.Min.Pose;
Pose pose1 = closestPoses.Max.Pose;
float weight = EasingUtil.GetEasing(pose1.PoseEasingStyle, pose1.PoseEasingDirection, 1 - alpha);
CFrame lastCFrame = pose0.CFrame;
CFrame nextCFrame = pose1.CFrame;
Bone baseBone = boneLookup[node.Name];
CFrame c0 = baseBone.C0;
CFrame c1 = baseBone.C1;
CFrame interp = lastCFrame.lerp(nextCFrame, weight);
// some ugly manual fixes.
// todo: make this unnecessary :(
if (sequence.AvatarType == AvatarType.R6)
{
Vector3 pos = interp.p;
CFrame rot = interp - pos;
if (node.Name == "Torso")
{
float[] ang = interp.toEulerAnglesXYZ();
rot = CFrame.Angles(ang[0], ang[2], ang[1]);
pos = new Vector3(pos.x, pos.z, pos.y);
}
else if (node.Name.StartsWith("Right"))
{
pos *= new Vector3(-1, 1, 1);
}
if (node.Name.Contains("Arm") || node.Name.Contains("Leg"))
{
pos = new Vector3(pos.z, pos.y, pos.x);
}
if (sequence.Name == "Climb" && node.Name.Contains("Leg")) // https://www.youtube.com/watch?v=vfJ7DqyDl9w
{
pos += new Vector3(-.1f, 0, 0);
}
interp = new CFrame(pos) * rot;
}
else if (sequence.AvatarType == AvatarType.R15)
{
if (node.Name.Contains("UpperArm"))
{
Vector3 pos = interp.p;
CFrame rot = interp - pos;
float[] ang = rot.toEulerAnglesXYZ();
if (sequence.Name == "Climb" || sequence.Name == "Swim")
{
rot = CFrame.Angles(ang[0], -ang[2], -ang[1]);
}
interp = new CFrame(pos) * rot;
}
}
Bone bone = new Bone(node.Name, i, interp);
bone.Node = node;
bones.Add(bone);
}
boneKeyframes.Add(frame);
}
return(animWriter.BuildFile());
}
public static string Assemble(KeyframeSequence sequence, List <Bone> rig)
{
StudioMdlWriter animWriter = new StudioMdlWriter();
List <Keyframe> keyframes = new List <Keyframe>();
var boneLookup = new Dictionary <string, Bone>();
var nodes = animWriter.Nodes;
foreach (Bone bone in rig)
{
Node node = bone.Node;
if (node != null)
{
string boneName = node.Name;
if (!boneLookup.ContainsKey(boneName))
{
boneLookup.Add(boneName, bone);
}
nodes.Add(node);
}
}
foreach (Keyframe kf in sequence.GetChildrenOfClass <Keyframe>())
{
Pose rootPart = kf.FindFirstChild <Pose>("HumanoidRootPart");
if (rootPart != null)
{
// We don't need the rootpart for this.
foreach (Pose subPose in rootPart.GetChildrenOfClass <Pose>())
{
subPose.Parent = kf;
}
rootPart.Destroy();
}
kf.Time /= sequence.TimeScale;
keyframes.Add(kf);
}
keyframes.Sort(0, keyframes.Count, sorter);
Keyframe lastKeyframe = keyframes[keyframes.Count - 1];
float fLength = lastKeyframe.Time;
int frameCount = ToFrameRate(fLength);
// As far as I can tell, models in Source require you to store poses for every
// single frame, so I need to fill in the gaps with interpolated pose CFrames.
var keyframeMap = new Dictionary <int, Dictionary <string, Pose> >();
foreach (Keyframe kf in keyframes)
{
int frame = ToFrameRate(kf.Time);
var poses = GatherPoses(kf);
var poseMap = poses.ToDictionary(pose => pose.Name);
keyframeMap[frame] = poseMap;
}
// Make sure there are no holes in the data.
for (int i = 0; i < frameCount; i++)
{
if (!keyframeMap.ContainsKey(i))
{
var emptyState = new Dictionary <string, Pose>();
keyframeMap.Add(i, emptyState);
}
}
List <BoneKeyframe> boneKeyframes = animWriter.Skeleton;
for (int i = 0; i < frameCount; i++)
{
BoneKeyframe frame = new BoneKeyframe(i);
List <Bone> bones = frame.Bones;
if (sequence.AvatarType == AvatarType.R15)
{
frame.BaseRig = rig;
frame.DeltaSequence = true;
}
foreach (Node node in nodes)
{
PosePair closestPoses = GetClosestPoses(keyframeMap, i, node.Name);
float min = closestPoses.Min.Frame;
float max = closestPoses.Max.Frame;
float alpha = (min == max ? 0 : (i - min) / (max - min));
Pose pose0 = closestPoses.Min.Pose;
Pose pose1 = closestPoses.Max.Pose;
CFrame lastCFrame = pose0.CFrame;
CFrame nextCFrame = pose1.CFrame;
Bone baseBone = boneLookup[node.Name];
CFrame interp = lastCFrame.Lerp(nextCFrame, alpha);
// Make some patches to the interpolation offsets. Unfortunately I can't
// identify any single fix that I can apply to each joint, so I have to get crafty.
// At some point in the future, I want to find a more practical solution for this problem,
// but it is extremely difficult to isolate if any single solution exists.
if (sequence.AvatarType == AvatarType.R6)
{
Vector3 pos = interp.Position;
CFrame rot = interp - pos;
if (node.Name == "Torso")
{
// Flip the YZ axis of the Torso.
float[] ang = interp.ToEulerAnglesXYZ();
rot = CFrame.Angles(ang[0], ang[2], ang[1]);
pos = new Vector3(pos.X, pos.Z, pos.Y);
}
else if (node.Name.StartsWith("Right"))
{
// X-axis is inverted for the right arm/leg.
pos *= new Vector3(-1, 1, 1);
}
if (node.Name.EndsWith("Arm") || node.Name.EndsWith("Leg"))
{
// Rotate position offset of the arms & legs 90* counter-clockwise.
pos = new Vector3(-pos.Z, pos.Y, pos.X);
}
if (node.Name != "Head")
{
rot = rot.Inverse();
}
interp = new CFrame(pos) * rot;
}
else if (sequence.AvatarType == AvatarType.R15)
{
float[] ang = interp.ToEulerAnglesXYZ();
// Cancel out the rotations
interp *= CFrame.Angles(-ang[0], -ang[1], -ang[2]);
// Patch the Y-axis
PatchAngles(ref interp, 1, ang);
// Patch the Z-axis
PatchAngles(ref interp, 2, ang);
// Patch the X-axis
PatchAngles(ref interp, 0, ang);
}
Bone bone = new Bone(node, interp);
bones.Add(bone);
}
boneKeyframes.Add(frame);
}
return(animWriter.BuildFile());
}
internal override void Draw(ref DeviceContext context, ref Camera camera)
{
var adornee = Adornee;
if (adornee == null || Visible == false)
{
return;
}
PixHelper.BeginEvent(Color.Zero, "ArcHandles");
Renderer.AdornSelfLitPass.Use(ref context);
var adorneeCF = adornee.CFrame;
var adorneeSize = adornee.Size;
GetDistanceToNormals(10, 0.6f, ref adorneeCF, ref adorneeSize, ref _scales);
var radius = adorneeSize.max() * 0.75f;
for (int i = 0; i < 6; i++)
{
var sphere = _spheres[i];
var normalId = (NormalId)i;
sphere.CFrame = adorneeCF + (Vector3.FromNormalId(normalId) * radius);
sphere.Size = new Vector3(_scales[i]);
sphere.UpdateRenderData();
sphere.Draw(ref context, ref camera);
}
for (int i = 0; i < 3; i++)
{
CFrame angle;
if (i == 0)
{
angle = CFrame.Identity;
}
else if (i == 1)
{
angle = CFrame.Angles(0, Mathf.Pi / 2, 0);
}
else
{
angle = CFrame.Identity;
}
for (int j = 0; j < _segments; j++)
{
var cylinder = _cylinders[i * j];
cylinder.Size = _cylinderSize * (radius / 256);
var sine = Mathf.Sin((360 / _segments + 360 / _segments * j) / (180 / Mathf.Pi));
var cosine = Mathf.Cos((360 / _segments + 360 / _segments * j) / (180 / Mathf.Pi));
var cframe = (adorneeCF) + new Vector3(radius * sine, 0, radius * cosine);
cylinder.CFrame = cframe *
CFrame.Angles(0, (360 / _segments + 360 / _segments * j) / (180 / Mathf.Pi),
Mathf.Pi / 2);
cylinder.UpdateRenderData();
cylinder.Draw(ref context, ref camera);
}
}
PixHelper.EndEvent();
}
private void Update(double dt)
{
if (Player?.IsLocalPlayer != true)
{
return;
}
float angleY = 0;
var forward = InputService.Service.IsKeyDown(Key.W);
var backward = InputService.Service.IsKeyDown(Key.S);
var left = InputService.Service.IsKeyDown(Key.A);
var right = InputService.Service.IsKeyDown(Key.D);
var moved = true;
if (forward && left && !right)
{
angleY = 45;
}
else if (forward && right && !left)
{
angleY = -45;
}
else if (forward)
{
angleY = 0;
}
else if (backward && left && !right)
{
angleY = 180 + -45;
}
else if (backward && right && !left)
{
angleY = 180 + 45;
}
else if (backward)
{
angleY = 180;
}
else if (left && !right)
{
angleY = 90;
}
else if (right && !left)
{
angleY = -90;
}
else
{
moved = false;
}
angleY *= Mathf.Deg2Rad;
BulletSharp.Math.Vector3 linearVelocity;
if (moved)
{
var cameraCF = Game.FocusedCamera?.CFrame ?? CFrame.Identity;
var moveCF = cameraCF * CFrame.Angles(0, angleY, 0);
var moveDir = (moveCF.lookVector * _walkSpeed);
MoveDirection = new Vector3(moveDir.X, 0, moveDir.Y).unit;
linearVelocity = new BulletSharp.Math.Vector3(moveDir.X, 0, moveDir.Z);
}
else
{
linearVelocity = BulletSharp.Math.Vector3.Zero;
MoveDirection = Vector3.Zero;
}
lock (PhysicsSimulation.Locker)
{
RigidBody.LinearVelocity = linearVelocity;
}
}
