box2djsの作法 その12

概要

box2djsの作法、調べてみた。
drone、やってみた。
pid制御で、8の字飛行、やってみた。

写真

サンプルコード

var Agent3 = function() {
	this.err2 = 0;
  this.err = 0;
  this.P = 0;
  this.I = 0;
  this.D = 0;
};
Agent3.prototype.get_action = function(setPoint, input) {
  var Kp = 100;
  var Ki = 22;
  var Kd = 22;
  var dt = 0.01;
	this.P = setPoint - input;
  this.I += this.P * dt;
  this.D = (this.P - this.err2) / dt;
	var u = Kp * this.P + Ki * this.I + Kd * this.D;
	this.err2 = this.err;
	this.err = this.P;
	return u;
}

function getDistance(x, y, x2, y2) {
  var distance = Math.sqrt((x2 - x) * (x2 - x) + (y2 - y) * (y2 - y));
  return distance;
}
function drawWorld(world, context) {
	for (var b = world.m_bodyList; b; b = b.m_next)
  {
		for (var s = b.GetShapeList(); s != null; s = s.GetNext()) 
    {
			drawShape(s, context);
		}
	}
}
function drawShape(shape, context) {
	context.strokeStyle = '#ff0000';
	context.beginPath();
	switch (shape.m_type) 
  {
	case b2Shape.e_polyShape:
		var poly = shape;
		var tV = b2Math.AddVV(poly.m_position, b2Math.b2MulMV(poly.m_R, poly.m_vertices[0]));
		context.moveTo(tV.x, tV.y);
		for (var i = 0; i < poly.m_vertexCount; i++)
    {
			var v = b2Math.AddVV(poly.m_position, b2Math.b2MulMV(poly.m_R, poly.m_vertices[i]));
			context.lineTo(v.x, v.y);
		}
		context.lineTo(tV.x, tV.y);
	break;
	}
	context.moveTo(50, 100);
	context.lineTo(51, 101);
	context.moveTo(250, 100);
	context.lineTo(251, 101);
	context.moveTo(50, 200);
	context.lineTo(51, 201);
	context.moveTo(250, 200);
	context.lineTo(251, 201);
	context.stroke();
}

var worldAABB = new b2AABB();			
worldAABB.minVertex.Set(-1000, -1000);
worldAABB.maxVertex.Set(1000, 1000);
var gravity = new b2Vec2(0, 10);	
var doSleep = true;					
var world = new b2World(worldAABB, gravity, doSleep);

var boxSd = new b2BoxDef();
boxSd.density = 1.0;
boxSd.extents.Set(30, 12);
var boxBd = new b2BodyDef();
boxBd.AddShape(boxSd);
boxBd.position.Set(150, 100);
var box = world.CreateBody(boxBd);

var context = document.querySelector('#c').getContext('2d');
var timeStep = 1.0 / 60;
var iteration = 1;
var state = 4;
var agent0 = new Agent3;
var agent1 = new Agent3;
var point1 = {
  'x': 50,
  'y': 100
};
var point3 = {
  'x': 50,
  'y': 200
};
var point2 = {
  'x': 250,
  'y': 100
};
var point4 = {
  'x': 250,
  'y': 200
};
function to(point) {
  var x = box.m_position.x;
  var y = box.m_position.y;
	var position = box.GetCenterPosition().Copy();  
  var u0 = agent0.get_action(point.x, x);
  var u1 = agent1.get_action(point.y, y);
	box.ApplyImpulse(new b2Vec2(u0, u1), position);
}
setInterval(function() {
	switch (state)
  {
  case 4:        
    dis = getDistance(point4.x, point4.y, box.m_position.x, box.m_position.y);
    if (dis < 5)
    {
      state = 3;
      break;
    }
    to(point4);
  break;
  case 3:
    dis = getDistance(point3.x, point3.y, box.m_position.x, box.m_position.y);
    if (dis < 5)
    {
      state = 2;
      break;
    }
    to(point3);
  break;
  case 2:
    dis = getDistance(point2.x, point2.y, box.m_position.x, box.m_position.y);
    if (dis < 5)
    {
      state = 1;
      break;
    }
    to(point2);
  break;
  case 1:        
    dis = getDistance(point1.x, point1.y, box.m_position.x, box.m_position.y);
    if (dis < 5)
    {
      state = 4;
      break;
    }
    to(point1);
  break;         
  }
	context.clearRect(0, 0, 400, 300);
	world.Step(timeStep, iteration);
	drawWorld(world, context);
	var str = "x: " + box.m_position.x +
			"<br>y: " + box.m_position.y +
      "<br>distance: " + dis +
			"<br>state: " + state;
  document.getElementById('helloWorld').innerHTML = str;
}, 60);

成果物

以上。