Robotics (original) (raw)

. . .
Manipulator::Manipulator()
{
world = DirectX::XMMatrixIdentity();
if (Setup(6, 6, D3D11_USAGE_DYNAMIC, D3D_PRIMITIVE_TOPOLOGY_LINELIST) == false) return;
for (size_t i = 0; i < 6; i++)
{
vertex[i].position = DirectX::XMFLOAT3(0.0f, 0.0f, 0.0f);
index[i] = i;
}
//_________________________________________________ Link 1 (Red)
vertex[0].color = DirectX::XMFLOAT4(1.0f, 0.0f, 0.0f, 1.0f);
vertex[1].color = DirectX::XMFLOAT4(1.0f, 0.0f, 0.0f, 1.0f);
//_________________________________________________ Link 2 (Green)
vertex[2].color = DirectX::XMFLOAT4(0.0f, 1.0f, 0.0f, 1.0f);
vertex[3].color = DirectX::XMFLOAT4(0.0f, 1.0f, 0.0f, 1.0f);
//_________________________________________________ Link 3 (Blue)
vertex[4].color = DirectX::XMFLOAT4(0.0f, 0.0f, 1.0f, 1.0f);
vertex[5].color = DirectX::XMFLOAT4(0.0f, 0.0f, 1.0f, 1.0f);
ComputeH();
}

Manipulator::~Manipulator()
{
}

void Manipulator::ChangeD1(ID3D11DeviceContext* deviceContext, double delta)
{
d1 += delta;
ComputeH();
this->DynamicUpdate(deviceContext, true, false);
}

void Manipulator::ChangeD2(ID3D11DeviceContext* deviceContext, double delta)
{
d2 += delta;
ComputeH();
this->DynamicUpdate(deviceContext, true, false);
}

void Manipulator::ChangeD3(ID3D11DeviceContext* deviceContext, double delta)
{
d3 += delta;
ComputeH();
this->DynamicUpdate(deviceContext, true, false);
}

void Manipulator::ComputeH()
{
Math::Oper::DenavitD(90.0, 90.0, 0.0, 4.0+d1, H01);
Math::Oper::DenavitD(90.0, -90.0, 0.0, 8.0+d2, H12);
Math::Oper::DenavitD(0.0, 0.0, 0.0, 5.0+d3, H23);
Math::Oper::Product(H01, H12, H02); // H02 = H01*H12
Math::Oper::Product(H02, H23, H03); // H03 = H02*H23
//
if (this->GetVertexCount() == 0) return;
//_________________________________________________ Link 1
vertex[0].position = DirectX::XMFLOAT3(0.0f, 0.0f, 0.0f);
vertex[1].position = DX11::Geometry::HomegeneousTransformation(H01, 0.0f, 0.0f, 0.0f);
//_________________________________________________ Link 2
vertex[2].position = vertex[1].position;
vertex[3].position = DX11::Geometry::HomegeneousTransformation(H02, 0.0f, 0.0f, 0.0f);
//_________________________________________________ Link 3
vertex[4].position = vertex[3].position;
vertex[5].position = DX11::Geometry::HomegeneousTransformation(H03, 0.0f, 0.0f, 0.0f);
}

void Manipulator::Turn(Sys::Stopwatch& stopWatch)
{
const FLOAT delta = (FLOAT)stopWatch.GetSeconds();
//________________________________________________________ 1. Rotate, translate and scale
angleY += (0.5f*delta);
world = DirectX::XMMatrixRotationY(angleY);
if (angleY > DirectX::XM_2PI) angleY -= DirectX::XM_2PI;
}