Commit 5121a394 authored by amandaghassaei's avatar amandaghassaei

added ang velocity calc

parent 05477888
......@@ -5,9 +5,9 @@
define(['underscore', 'backbone', 'threeModel', 'lattice', 'plist', 'emWire', 'GPUMath', "text!simulation/shaders/vertexShader.js",
"text!simulation/function/EM/shaders/velocityCalcShader.js", "text!simulation/shaders/packToBytesShader.js", "text!simulation/function/EM/shaders/positionCalcShader.js",
"text!simulation/function/EM/shaders/quaternionCalcShader.js", 'emSimCell'],
"text!simulation/function/EM/shaders/angVelocityCalcShader.js", 'emSimCell'],
function(_, Backbone, three, lattice, plist, EMWire, gpuMath, vertexShader, velocityCalcShader, packToBytesShader,
positionCalcShader, quaternionCalcShader) {
positionCalcShader, angVelocityCalcShader) {
var EMSimLattice = Backbone.Model.extend({
......@@ -59,7 +59,6 @@ define(['underscore', 'backbone', 'threeModel', 'lattice', 'plist', 'emWire', 'G
this.cellsArrayMapping = new Int16Array(textureSize*4);//holds lattice index of cell (for rendering from texture)
//todo add moment of inertia
this.mass = new Float32Array(textureSize*4);//first element is mass, second element in fixed, third element is moment of inertia
this.neighborsXMapping = new Float32Array(textureSize*8);//-1 equals no neighb
......@@ -101,6 +100,8 @@ define(['underscore', 'backbone', 'threeModel', 'lattice', 'plist', 'emWire', 'G
self.mass[rgbaIndex] = self._calcCellMass(cell);
self.mass[rgbaIndex+1] = 0;//indicated a cell is present
var latticePitch = lattice.getPitch();
self.mass[rgbaIndex+2] = 1/6*self.mass[rgbaIndex]*latticePitch.x*latticePitch.x;//moment of inertia for a cube
self.cellsIndexMapping[x][y][z] = index;
......@@ -279,6 +280,7 @@ define(['underscore', 'backbone', 'threeModel', 'lattice', 'plist', 'emWire', 'G
//programs
//gpuMath.createProgram("quaternionCalc", vertexShader, quaternionCalcShader);
//gpuMath.createProgram("angVelocityCalc", vertexShader, quaternionCalcShader);
//gpuMath.setUniformForProgram("angVelocityCalc", "u_lastTranslation", 0, "1i");
//gpuMath.setUniformForProgram("angVelocityCalc", "u_lastQuaternion", 1, "1i");
......@@ -293,19 +295,25 @@ define(['underscore', 'backbone', 'threeModel', 'lattice', 'plist', 'emWire', 'G
//gpuMath.setUniformForProgram("angVelocityCalc", "u_wiresMetaLength", this.wiresMeta.length/4, "1f");
//gpuMath.setUniformForProgram("angVelocityCalc", "u_time", 0, "1f");
//
//gpuMath.createProgram("quaternionCalc", vertexShader, quaternionCalcShader);
//gpuMath.setUniformForProgram("quaternionCalc", "u_lastTranslation", 0, "1i");
//gpuMath.setUniformForProgram("quaternionCalc", "u_lastQuaternion", 1, "1i");
//gpuMath.setUniformForProgram("quaternionCalc", "u_mass", 2, "1i");
//gpuMath.setUniformForProgram("quaternionCalc", "u_neighborsXMapping", 3, "1i");
//gpuMath.setUniformForProgram("quaternionCalc", "u_neighborsYMapping", 4, "1i");
//gpuMath.setUniformForProgram("quaternionCalc", "u_compositeKs", 5, "1i");
//gpuMath.setUniformForProgram("quaternionCalc", "u_wires", 6, "1i");
//gpuMath.setUniformForProgram("quaternionCalc", "u_wiresMeta", 7, "1i");
//gpuMath.setUniformForProgram("quaternionCalc", "u_textureDim", [textureDim, textureDim], "2f");
//gpuMath.setUniformForProgram("quaternionCalc", "u_latticePitch", [latticePitch.x, latticePitch.y, latticePitch.z], "3f");
//gpuMath.setUniformForProgram("quaternionCalc", "u_wiresMetaLength", this.wiresMeta.length/4, "1f");
//gpuMath.setUniformForProgram("quaternionCalc", "u_time", 0, "1f");
gpuMath.createProgram("angVelocityCalc", vertexShader, angVelocityCalcShader);
gpuMath.setUniformForProgram("angVelocityCalc", "u_lastAngVelocity", 0, "1i");
gpuMath.setUniformForProgram("angVelocityCalc", "u_lastVelocity", 1, "1i");
gpuMath.setUniformForProgram("angVelocityCalc", "u_lastTranslation", 2, "1i");
gpuMath.setUniformForProgram("angVelocityCalc", "u_mass", 3, "1i");
gpuMath.setUniformForProgram("angVelocityCalc", "u_neighborsXMapping", 4, "1i");
gpuMath.setUniformForProgram("angVelocityCalc", "u_neighborsYMapping", 5, "1i");
gpuMath.setUniformForProgram("angVelocityCalc", "u_compositeKs", 6, "1i");
gpuMath.setUniformForProgram("angVelocityCalc", "u_compositeDs", 7, "1i");
gpuMath.setUniformForProgram("angVelocityCalc", "u_lastQuaternion", 8, "1i");
gpuMath.setUniformForProgram("angVelocityCalc", "u_wires", 9, "1i");
gpuMath.setUniformForProgram("angVelocityCalc", "u_wiresMeta", 10, "1i");
gpuMath.setUniformForProgram("angVelocityCalc", "u_textureDim", [textureDim, textureDim], "2f");
gpuMath.setUniformForProgram("angVelocityCalc", "u_multiplier", 1/(plist.allUnitTypes[lattice.getUnits()].multiplier), "1f");
gpuMath.setUniformForProgram("angVelocityCalc", "u_latticePitch", [latticePitch.x, latticePitch.y, latticePitch.z], "3f");
gpuMath.setUniformForProgram("angVelocityCalc", "u_wiresMetaLength", this.wiresMeta.length/4, "1f");
gpuMath.setUniformForProgram("angVelocityCalc", "u_time", 0, "1f");
gpuMath.createProgram("velocityCalc", vertexShader, velocityCalcShader);
gpuMath.setUniformForProgram("velocityCalc", "u_lastVelocity", 0, "1i");
......@@ -551,6 +559,10 @@ define(['underscore', 'backbone', 'threeModel', 'lattice', 'plist', 'emWire', 'G
gpuMath.setUniformForProgram("velocityCalc", "u_friction", friction, "1f");
gpuMath.setProgram("positionCalc");
gpuMath.setUniformForProgram("positionCalc", "u_dt", dt, "1f");
gpuMath.setProgram("angVelocityCalc");
gpuMath.setUniformForProgram("angVelocityCalc", "u_dt", dt, "1f");
//gpuMath.setProgram("quaternionCalc");
//gpuMath.setUniformForProgram("quaternionCalc", "u_dt", dt, "1f");
},
iter: function(time, runConstants, shouldRender){
......@@ -639,7 +651,7 @@ define(['underscore', 'backbone', 'threeModel', 'lattice', 'plist', 'emWire', 'G
var mass = this.mass[rgbaIndex];
if (mass == 0) continue;
var force = [mass*gravity.x, mass*gravity.y, mass*gravity.z];//translational force
var I = 1/6*mass*latticePitch[0]*latticePitch[0];//moment of inertia for a cube
var I = this.mass[rgbaIndex+2];//moment of inerita
var rForce = [0,0,0];//rotational force
......
......@@ -9,10 +9,9 @@ uniform float u_multiplier;
uniform vec3 u_latticePitch;
uniform float u_wiresMetaLength;
uniform float u_time;
uniform float u_groundHeight;
uniform float u_friction;
uniform sampler2D u_angularVelocity;
uniform sampler2D u_lastVelocity;
uniform sampler2D u_lastTranslation;
uniform sampler2D u_mass;
......@@ -20,11 +19,12 @@ uniform sampler2D u_neighborsXMapping;
uniform sampler2D u_neighborsYMapping;
uniform sampler2D u_compositeKs;
uniform sampler2D u_compositeDs;
uniform sampler2D u_originalPosition;
uniform sampler2D u_lastQuaternion;
uniform sampler2D u_wires;
uniform sampler2D u_wiresMeta;
vec3 applyQuaternion(vec3 vector, vec4 quaternion) {
float x = vector[0];
......@@ -44,8 +44,99 @@ vec3 applyQuaternion(vec3 vector, vec4 quaternion) {
float iw = - qx * x - qy * y - qz * z;
// calculate result * inverse quat
return vec3(ix * qw + iw * - qx + iy * - qz - iz * - qy, iy * qw + iw * - qy + iz * - qx - ix * - qz,
iz * qw + iw * - qz + ix * - qy - iy * - qx);
return vec3(ix * qw + iw * - qx + iy * - qz - iz * - qy, iy * qw + iw * - qy + iz * - qx - ix * - qz, iz * qw + iw * - qz + ix * - qy - iy * - qx);
}
vec4 quaternionFromUnitVectors(vec3 vFrom, vec3 vTo) {
vec3 v1 = vec3(0);
float r = dot(vFrom, vTo)+1.0;
if (r < 0.000001) {
r = 0.0;
if (abs(vFrom[0]) > abs(vFrom[2])) v1 = vec3(-vFrom[1], vFrom[0], 0.0);
else v1 = vec3(0.0, - vFrom[2], vFrom[1]);
} else v1 = cross(vFrom, vTo);
return normalize(vec4(v1, r));
}
mat4 makeRotationMatrixFromQuaternion(vec4 q) {
mat4 te;
float x = q[0];
float y = q[1];
float z = q[2];
float w = q[3];
float x2 = x + x;
float y2 = y + y;
float z2 = z + z;
float xx = x * x2;
float xy = x * y2;
float xz = x * z2;
float yy = y * y2;
float yz = y * z2;
float zz = z * z2;
float wx = w * x2;
float wy = w * y2;
float wz = w * z2;
te[0] = vec4(1.0 - ( yy + zz ), xy - wz, xz + wy, 0);
te[1] = vec4(xy + wz, 1.0 - ( xx + zz ), yz - wx, 0);
te[2] = vec4(xz - wy, yz + wx, 1.0 - ( xx + yy ), 0);
te[3] = vec4(0, 0, 0, 1);
return te;
}
vec3 setFromRotationMatrix(mat4 te) {
// assumes the upper 3x3 of m is a pure rotation matrix (i.e, unscaled)
float m11 = te[0][0];
float m12 = te[0][1];
float m13 = te[0][2];
float m21 = te[1][0];
float m22 = te[1][1];
float m23 = te[1][2];
float m31 = te[2][0];
float m32 = te[2][1];
float m33 = te[2][2];
if (abs(m13) < 0.99999){
return vec3(atan(-m23, m33), asin(clamp(m13, -1.0, 1.0)), atan(-m12, m11));
}
return vec3(atan(m32, m22), asin(clamp(m13, -1.0, 1.0)), 0);
}
vec3 eulerFromQuaternion(vec4 q){
return setFromRotationMatrix(makeRotationMatrixFromQuaternion(q));
}
vec4 quaternionFromEuler(vec3 euler) {
float c1 = cos(euler[0]/2.0);
float c2 = cos(euler[1]/2.0);
float c3 = cos(euler[2]/2.0);
float s1 = sin(euler[0]/2.0);
float s2 = sin(euler[1]/2.0);
float s3 = sin(euler[2]/2.0);
return vec4(s1 * c2 * c3 + c1 * s2 * s3, c1 * s2 * c3 - s1 * c2 * s3, c1 * c2 * s3 + s1 * s2 * c3, c1 * c2 * c3 - s1 * s2 * s3);
}
vec4 quaternionFromAxisAngle(vec3 axis, float angle) {
float halfAngle = angle/2.0;
float s = sin(halfAngle);
return vec4(s*axis, cos(halfAngle));
}
vec4 multiplyQuaternions(vec4 a, vec4 b){
float qax = a[0];
float qay = a[1];
float qaz = a[2];
float qaw = a[3];
float qbx = b[0];
float qby = b[1];
float qbz = b[2];
float qbw = b[3];
return vec4(qax * qbw + qaw * qbx + qay * qbz - qaz * qby, qay * qbw + qaw * qby + qaz * qbx - qax * qbz,
qaz * qbw + qaw * qbz + qax * qby - qay * qbx, qaw * qbw - qax * qbx - qay * qby - qaz * qbz);
}
float neighborSign(float i){
......@@ -81,7 +172,7 @@ float getActuatorVoltage(float wireIndex){
float frequency = wireMeta[1];
float period = 1.0/frequency;
float phase = wireMeta[2];
float currentPhase = mod(u_time+phase*period, period)/period;
float currentPhase = mod(u_time + phase*period, period)/period;
if (type == 0){
return 0.5*sin(2.0*M_PI*currentPhase);
}
......@@ -178,37 +269,26 @@ void main(){
vec2 fragCoord = gl_FragCoord.xy;
vec2 scaledFragCoord = fragCoord/u_textureDim;
float isFixed = texture2D(u_mass, scaledFragCoord).y;
vec3 massData = texture2D(u_mass, scaledFragCoord).xyz;
float isFixed = massData.y;
if (isFixed < 0.0 || isFixed == 1.0){//no cell or is fixed
gl_FragColor = vec4(0, 0, 0, 0);
return;
}
float mass = texture2D(u_mass, scaledFragCoord).x;
vec3 force = u_gravity*mass;
float mass = massData.x;
float momOfInertia = massData.z;
vec3 torque = vec3(0.0, 0.0, 0.0);
vec3 translation = texture2D(u_lastTranslation, scaledFragCoord).xyz;
vec3 velocity = texture2D(u_lastVelocity, scaledFragCoord).xyz;
vec4 quaternion = texture2D(u_lastQuaternion, scaledFragCoord);
vec3 angVelocity = texture2D(u_angularVelocity, scaledFragCoord).xyz;
vec4 wiring = texture2D(u_wires, scaledFragCoord);
bool isActuator = wiring[0] < -0.5;//-1
//simple collision
float zPosition = texture2D(u_originalPosition, scaledFragCoord).z + translation.z*u_multiplier - u_groundHeight;
float collisionK = 1.0;
if (zPosition < 0.0) {
float normalForce = -zPosition*collisionK-velocity.z*collisionK/10.0;
force.z += normalForce;
if (u_friction > 0.5){
float mu = 10.0;
if (velocity.x > 0.0) force.x -= mu * normalForce;
else if (velocity.x < 0.0) force.x += mu * normalForce;
if (velocity.y > 0.0) force.y -= mu * normalForce;
else if (velocity.y < 0.0) force.y += mu * normalForce;
}
}
for (float i=0.0;i<2.0;i+=1.0){
float xIndex = 2.0*(fragCoord.x-0.5) + 0.5;
......@@ -261,9 +341,13 @@ void main(){
//else if (neighborAxis == 1) actuatedD[1] *= 1.0+actuation;
//else if (neighborAxis == 2) actuatedD[2] *= 1.0+actuation;
vec2 kIndex = vec2(((fragCoord.x-0.5)*12.0 + 2.0*(i*3.0+float(j)) + 0.5)/(u_textureDim.x*12.0), scaledFragCoord.y);
float kPosition = ((fragCoord.x-0.5)*12.0 + 2.0*(i*3.0+float(j)) + 0.5);
vec2 kIndex = vec2(kPosition/(u_textureDim.x*12.0), scaledFragCoord.y);
vec3 translationalK = texture2D(u_compositeKs, kIndex).xyz;
vec3 translationalD = texture2D(u_compositeDs, kIndex).xyz;
kIndex.x = (kPosition+1.0)/(u_textureDim.x*12.0);
vec3 rotationalK = texture2D(u_compositeKs, kIndex).xyz;
vec3 rotationalD = texture2D(u_compositeDs, kIndex).xyz;
vec4 averageQuaternion = averageQuaternions(quaternion, neighborQuaternion);
vec4 averageQuaternionInverse = invertQuaternion(averageQuaternion);
......@@ -276,9 +360,18 @@ void main(){
vec3 _force = translationalK*translationalDeltaXYZ + translationalD*velocityDeltaXYZ;
//convert _force vector back into world reference frame
_force = applyQuaternion(_force, averageQuaternion);
force += _force;
////translational forces cause rotation in cell - convert to cell reference frame
torque += cross(halfNominalD, applyQuaternion(_force, invertQuaternion(quaternion)));//cellHalfNominalD = lever arm
//bending and torsion
vec4 quaternionDiff = multiplyQuaternions(invertQuaternion(quaternion), neighborQuaternion);
vec3 diffEuler = eulerFromQuaternion(quaternionDiff);
torque += 0.00001*rotationalK*diffEuler;// + rotationalD*(neighborAngVelocity[_axis]-angVelocity[_axis]);
}
}
gl_FragColor = vec4(force, 0);
angVelocity += torque/momOfInertia;
gl_FragColor = vec4(angVelocity, 0);
}
\ No newline at end of file
#define M_PI 3.1415926535897932384626433832795
precision mediump float;
uniform vec2 u_textureDim;
uniform vec3 u_latticePitch;
uniform float u_wiresMetaLength;
uniform float u_time;
uniform sampler2D u_lastTranslation;
uniform sampler2D u_fixed;
uniform sampler2D u_neighborsXMapping;
uniform sampler2D u_neighborsYMapping;
uniform sampler2D u_lastQuaternion;
uniform sampler2D u_compositeKs;
uniform sampler2D u_wires;
uniform sampler2D u_wiresMeta;
vec3 applyQuaternion(vec3 vector, vec4 quaternion) {
float x = vector[0];
float y = vector[1];
float z = vector[2];
float qx = quaternion[0];
float qy = quaternion[1];
float qz = quaternion[2];
float qw = quaternion[3];
// calculate quat * vector
float ix = qw * x + qy * z - qz * y;
float iy = qw * y + qz * x - qx * z;
float iz = qw * z + qx * y - qy * x;
float iw = - qx * x - qy * y - qz * z;
// calculate result * inverse quat
return vec3(ix * qw + iw * - qx + iy * - qz - iz * - qy, iy * qw + iw * - qy + iz * - qx - ix * - qz, iz * qw + iw * - qz + ix * - qy - iy * - qx);
}
vec4 quaternionFromUnitVectors(vec3 vFrom, vec3 vTo) {
vec3 v1 = vec3(0);
float r = dot(vFrom, vTo)+1.0;
if (r < 0.000001) {
r = 0.0;
if (abs(vFrom[0]) > abs(vFrom[2])) v1 = vec3(-vFrom[1], vFrom[0], 0.0);
else v1 = vec3(0.0, - vFrom[2], vFrom[1]);
} else v1 = cross(vFrom, vTo);
return normalize(vec4(v1, r));
}
mat4 makeRotationMatrixFromQuaternion(vec4 q) {
mat4 te;
float x = q[0];
float y = q[1];
float z = q[2];
float w = q[3];
float x2 = x + x;
float y2 = y + y;
float z2 = z + z;
float xx = x * x2;
float xy = x * y2;
float xz = x * z2;
float yy = y * y2;
float yz = y * z2;
float zz = z * z2;
float wx = w * x2;
float wy = w * y2;
float wz = w * z2;
te[0] = vec4(1.0 - ( yy + zz ), xy - wz, xz + wy, 0);
te[1] = vec4(xy + wz, 1.0 - ( xx + zz ), yz - wx, 0);
te[2] = vec4(xz - wy, yz + wx, 1.0 - ( xx + yy ), 0);
te[3] = vec4(0, 0, 0, 1);
return te;
}
vec3 setFromRotationMatrix(mat4 te) {
// assumes the upper 3x3 of m is a pure rotation matrix (i.e, unscaled)
float m11 = te[0][0];
float m12 = te[0][1];
float m13 = te[0][2];
float m21 = te[1][0];
float m22 = te[1][1];
float m23 = te[1][2];
float m31 = te[2][0];
float m32 = te[2][1];
float m33 = te[2][2];
if (abs(m13) < 0.99999){
return vec3(atan(-m23, m33), asin(clamp(m13, -1.0, 1.0)), atan(-m12, m11));
}
return vec3(atan(m32, m22), asin(clamp(m13, -1.0, 1.0)), 0);
}
vec3 eulerFromQuaternion(vec4 q){
return setFromRotationMatrix(makeRotationMatrixFromQuaternion(q));
}
vec4 quaternionFromEuler(vec3 euler) {
float c1 = cos(euler[0]/2.0);
float c2 = cos(euler[1]/2.0);
float c3 = cos(euler[2]/2.0);
float s1 = sin(euler[0]/2.0);
float s2 = sin(euler[1]/2.0);
float s3 = sin(euler[2]/2.0);
return vec4(s1 * c2 * c3 + c1 * s2 * s3, c1 * s2 * c3 - s1 * c2 * s3, c1 * c2 * s3 + s1 * s2 * c3, c1 * c2 * c3 - s1 * s2 * s3);
}
vec4 quaternionFromAxisAngle(vec3 axis, float angle) {
float halfAngle = angle/2.0;
float s = sin(halfAngle);
return vec4(s*axis, cos(halfAngle));
}
vec4 multiplyQuaternions(vec4 a, vec4 b){
float qax = a[0];
float qay = a[1];
float qaz = a[2];
float qaw = a[3];
float qbx = b[0];
float qby = b[1];
float qbz = b[2];
float qbw = b[3];
return vec4(qax * qbw + qaw * qbx + qay * qbz - qaz * qby, qay * qbw + qaw * qby + qaz * qbx - qax * qbz,
qaz * qbw + qaw * qbz + qax * qby - qay * qbx, qaw * qbw - qax * qbx - qay * qby - qaz * qbz);
}
float neighborSign(float i){
if (mod(i+0.001,2.0) < 0.5) return -1.0;
return 1.0;
}
vec3 neighborOffset(float i, int neighborAxis){
vec3 offset = vec3(0);
if (neighborAxis == 0) offset[0] = neighborSign(i)*u_latticePitch[0];
else if (neighborAxis == 1) offset[1] = neighborSign(i)*u_latticePitch[1];
else if (neighborAxis == 2) offset[2] = neighborSign(i)*u_latticePitch[2];
return offset;
}
int calcNeighborAxis(float i){
return int(floor(i/2.0+0.001));
}
int convertToInt(float num){
return int(floor(num+0.001));
}
float getActuatorVoltage(float wireIndex){
vec2 wireCoord = vec2(0.5, (floor(wireIndex+0.001)+0.5)/u_wiresMetaLength);
vec4 wireMeta = texture2D(u_wiresMeta, wireCoord);
int type = convertToInt(wireMeta[0]);
if (type == -1) {
//no signal connected
return 0.0;
}
float frequency = wireMeta[1];
float period = 1.0/frequency;
float phase = wireMeta[2];
float currentPhase = mod(u_time + phase*period, period)/period;
if (type == 0){
return 0.5*sin(2.0*M_PI*currentPhase);
}
if (type == 1){
float pwm = wireMeta[3];
if (currentPhase < pwm) return 0.5;
return -0.5;
}
if (type == 2){
if (wireMeta[3]>0.5) return 0.5-currentPhase;
return currentPhase-0.5;
}
if (type == 3){
if (currentPhase < 0.5) return currentPhase*2.0-0.5;
return 0.5-(currentPhase-0.5)*2.0;
}
return 0.0;
}
void main(){
vec2 fragCoord = gl_FragCoord.xy;
vec2 scaledFragCoord = fragCoord/u_textureDim;
float isFixed = texture2D(u_fixed, scaledFragCoord).x;
if (isFixed < 0.0 || isFixed == 1.0){//no cell or is fixed
gl_FragColor = vec4(0,0,0,1);
return;
}
vec3 translation = texture2D(u_lastTranslation, scaledFragCoord).xyz;
vec4 quaternion = texture2D(u_lastQuaternion, scaledFragCoord);
vec4 wiring = texture2D(u_wires, scaledFragCoord);
bool isActuator = wiring.x < -0.5;//-1
vec3 rTotal = vec3(0);
float rContrib = 0.0;
for (float i=0.0;i<2.0;i+=1.0){
float xIndex = 2.0*(fragCoord.x-0.5) + 0.5;
if (i>0.0) xIndex += 1.0;
vec2 mappingIndex = vec2(xIndex/(u_textureDim.x*2.0), scaledFragCoord.y);
vec3 neighborsXMapping = texture2D(u_neighborsXMapping, mappingIndex).xyz;
vec3 neighborsYMapping = texture2D(u_neighborsYMapping, mappingIndex).xyz;
vec3 compositeKs = texture2D(u_compositeKs, mappingIndex).xyz;
for (int j=0;j<3;j++){
if (neighborsXMapping[j] < 0.0) continue;//no neighbor
int neighborAxis = calcNeighborAxis(i*3.0+float(j));
vec2 neighborIndex = vec2(neighborsXMapping[j], neighborsYMapping[j]);
neighborIndex.x += 0.5;
neighborIndex.y += 0.5;
vec2 scaledNeighborIndex = neighborIndex/u_textureDim;
vec3 neighborTranslation = texture2D(u_lastTranslation, scaledNeighborIndex).xyz;
vec4 neighborQuaternion = texture2D(u_lastQuaternion, scaledNeighborIndex);
vec3 neighborEuler = eulerFromQuaternion(neighborQuaternion);
vec3 nominalD = neighborOffset(i*3.0+float(j), neighborAxis);
vec3 actuatedD = vec3(nominalD[0], nominalD[1], nominalD[2]);
float actuation = 0.0;
if (isActuator){
if (neighborAxis == 0 && wiring[1]>0.1){//>0
actuation += 0.3*getActuatorVoltage(wiring[1]-1.0);
} else if (neighborAxis == 1 && wiring[2]>0.1){
actuation += 0.3*getActuatorVoltage(wiring[2]-1.0);
} else if (neighborAxis == 2 && wiring[3]>0.1){
actuation += 0.3*getActuatorVoltage(wiring[3]-1.0);
}
}
vec4 neighborWiring = texture2D(u_wires, scaledNeighborIndex);
if (neighborWiring[0] < -0.5){
if (neighborAxis == 0 && neighborWiring[1]>0.1){
actuation += 0.3*getActuatorVoltage(neighborWiring[1]-1.0);
} else if (neighborAxis == 1 && neighborWiring[2]>0.1){
actuation += 0.3*getActuatorVoltage(neighborWiring[2]-1.0);
} else if (neighborAxis == 2 && neighborWiring[3]>0.1){
actuation += 0.3*getActuatorVoltage(neighborWiring[3]-1.0);
}
}
if (neighborAxis == 0) actuatedD[0] *= 1.0+actuation;
else if (neighborAxis == 1) actuatedD[1] *= 1.0+actuation;
else if (neighborAxis == 2) actuatedD[2] *= 1.0+actuation;
vec3 D = neighborTranslation-translation+nominalD;
vec3 halfNominalD = actuatedD*0.5;
vec3 rotatedHalfNomD = applyQuaternion(halfNominalD, quaternion);
vec3 neighbRotatedHalfNomD = applyQuaternion(halfNominalD, neighborQuaternion);
vec3 rotatedNominalD = rotatedHalfNomD + neighbRotatedHalfNomD;
float k = compositeKs[j];
//non-axial rotation
vec4 nonAxialRotation = quaternionFromUnitVectors(normalize(nominalD), normalize(D));
//axial rotation
vec3 axis = rotatedNominalD;//neighbRotatedHalfNomD
float angle = dot(neighborEuler, normalize(axis));
vec4 torsion = quaternionFromAxisAngle(normalize(nominalD), angle);
vec3 rotaionEuler = eulerFromQuaternion(multiplyQuaternions(nonAxialRotation, torsion));
rTotal += rotaionEuler*k;
rContrib += k;
}
}
gl_FragColor = quaternionFromEuler(rTotal/rContrib);
}
\ No newline at end of file
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment