package jel.j3d;
import com.sun.j3d.utils.geometry.ColorCube;
import com.sun.j3d.utils.geometry.Cone;
import com.sun.j3d.utils.universe.SimpleUniverse;
import java.awt.event.KeyEvent;
import java.awt.event.KeyListener;
import javax.media.j3d.Appearance;
import javax.media.j3d.BranchGroup;
import javax.media.j3d.ColoringAttributes;
import javax.media.j3d.Transform3D;
import javax.media.j3d.TransformGroup;
import javax.vecmath.Color3f;
import javax.vecmath.Quat4d;
import javax.vecmath.Vector3d;
/**
*
* @author Jex
*
* This is my first minimum implementation for a simple YAW PITCH ROLL program in java3d<br>
* I got the rotation algoritm idea from https://luckytoilet.wordpress.com/category/programming/ <br>https://en.wikipedia.org/wiki/Euler_angles
*
* Next version will be to decrease computations.
* Tested on Java 3D 1.5.1(x64)<br>
* (right hand 3d system)<br>
* jdk-jre 8 update 131<br>
* Windows 7, April 2017<br>
* jimakoskx@hotmail.com
*
*
*
*/
public class YawPitchRollRegulator {
static YawPitchRollRegulator ClassShower=new YawPitchRollRegulator();
static TransformGroup rotInitialtg=new TransformGroup();
static TransformGroup postg=new TransformGroup();
static TransformGroup rottg=new TransformGroup();
static TransformGroup followtg=new TransformGroup();
static Vector3d pos=new Vector3d();
public static void main(String[] args) {
//using above statics that can be deleted
postg.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
rottg.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
rotInitialtg.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
followtg.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
System.setProperty("sun.awt.noerasebackground", "true");
SimpleUniverse su=new SimpleUniverse();
Appearance appear=new Appearance();
appear.setColoringAttributes(new ColoringAttributes(new Color3f(0,0,1),ColoringAttributes.NICEST));
Cone movingCone=new Cone(0.5f,0.3f,appear);
Transform3D initialRotationOfCone=new Transform3D();
initialRotationOfCone.rotX(-Math.PI/2);
rotInitialtg.setTransform(initialRotationOfCone);
double ccr=0.2;
ColorCube followerCube=new ColorCube(ccr);
Transform3D initialPosOfCube=new Transform3D();
initialPosOfCube.setTranslation(new Vector3d(0,0,ccr*2));
followtg.setTransform(initialPosOfCube);
ColorCube cc2=new ColorCube(ccr/2);
BranchGroup bgMovingContent=new BranchGroup();
bgMovingContent.addChild(postg);
postg.addChild(rottg);
rottg.addChild(rotInitialtg);
rotInitialtg.addChild(movingCone);
rottg.addChild(followtg);
followtg.addChild(followerCube);
su.getCanvas().addKeyListener(new KeyListener() {
@Override
public void keyPressed(KeyEvent e) {
int keyCode=e.getKeyCode();
boolean iSControl=e.isControlDown();
double st=0.1;
if(keyCode==KeyEvent.VK_UP){
if(iSControl){pos.y+=st;Transform3D ntr=new Transform3D();ntr.setTranslation(pos);postg.setTransform(ntr);}
else{
ClassShower.addPitch();
rottg.setTransform(ClassShower.trans);
}
}
else if(keyCode==KeyEvent.VK_DOWN){
if(iSControl){pos.y-=st;Transform3D ntr=new Transform3D();ntr.setTranslation(pos);postg.setTransform(ntr);}
else{
ClassShower.addPitchMinus();
rottg.setTransform(ClassShower.trans);
}
}
else if(keyCode==KeyEvent.VK_LEFT){
if(iSControl){pos.x-=st;Transform3D ntr=new Transform3D();ntr.setTranslation(pos);postg.setTransform(ntr);}
else{
ClassShower.addRoll();
rottg.setTransform(ClassShower.trans);
}
}
else if(keyCode==KeyEvent.VK_RIGHT){
if(iSControl){pos.x+=st;Transform3D ntr=new Transform3D();ntr.setTranslation(pos);postg.setTransform(ntr);}
else{
ClassShower.addRollMinnus();
rottg.setTransform(ClassShower.trans);
}
}
else if(keyCode==KeyEvent.VK_A){//down
if(iSControl){pos.z-=st;Transform3D ntr=new Transform3D();ntr.setTranslation(pos);postg.setTransform(ntr);}
else{
ClassShower.addYaw();
rottg.setTransform(ClassShower.trans);
}
}
else if(keyCode==KeyEvent.VK_Z){//up
if(iSControl){pos.z+=st;Transform3D ntr=new Transform3D();ntr.setTranslation(pos);postg.setTransform(ntr);}
else{
ClassShower.addYawMinus();
rottg.setTransform(ClassShower.trans);
}
}
else if(keyCode==KeyEvent.VK_ENTER){ClassShower.resetYawPitchRoll(0,0,0);rottg.setTransform(ClassShower.trans);}
else if(keyCode==KeyEvent.VK_SPACE){//move
Point3d v=new Point3d(0,0,-0.2);
ClassShower.trans.transform(v);
pos.add(v);
Transform3D ntr=new Transform3D();ntr.setTranslation(pos);postg.setTransform(ntr);
}
}
public void keyTyped(KeyEvent e) {}
public void keyReleased(KeyEvent e) {}
});
su.getViewingPlatform().setNominalViewingTransform();
su.addBranchGraph(bgMovingContent);
su.getCanvas().requestFocus();
}
//Delete all above!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
/**
* Yaw Pitch Roll Values divided by 2
*/
public double x,y,z;
/**
* helping for speed (not allocating)
*/
double dx,dy,dz;
/**
* The main Quad that holds the Total Yaw Pitch Roll values
*/
public Quat4d q;
/**
* When you want to add a certain amount we have this items (not allocating)
*/
public Quat4d qx,qy,qz;
/**
* More quicker if we have the steps ready and just execute one Quad multiplication
*/
public Quat4d qxAdd,qyAdd,qzAdd;
public Quat4d qxMinus,qyMinus,qzMinus;
/**
* Every time that you change the values you must call from your code<br>
* setTransform(trans) .... (refresh)
*/
public Transform3D trans;
/**
* Constructs with zero Yaw Pitch Roll and with the default Step degrees
*/
public YawPitchRollRegulator(){
trans=new Transform3D();
q=new Quat4d(0,0,0,1);
qx=new Quat4d(0,0,0,1);
qy=new Quat4d(0,0,0,1);
qz=new Quat4d(0,0,0,1);
qxAdd=new Quat4d(0,0,0,1);
qyAdd=new Quat4d(0,0,0,1);
qzAdd=new Quat4d(0,0,0,1);
qxMinus=new Quat4d(0,0,0,1);
qyMinus=new Quat4d(0,0,0,1);
qzMinus=new Quat4d(0,0,0,1);
resetYawPitchRoll(0,0,0);
//ensuring Minus step is set
setAddStepsForYawPitchRoll(Math.toRadians(2),Math.toRadians(5), Math.toRadians(10));
}
//helping fnctions
public double pitch(){return x;}
public double yaw(){return y;}
public double roll(){return z;}
public double pitchDegrees(){return Math.toDegrees(x);}
public double yawDegrees(){return Math.toDegrees(y);}
public double rollDegrees(){return Math.toDegrees(z);}
public String toString(){
String out=getClass().getSimpleName()+" Yaw( "+yawDegrees()+" ) , Pitch( "+pitchDegrees()+" ) , Roll( "+rollDegrees()+" )";
return out;
}
//Thread thr=new Thread("Avoiding Overflows on the Air !"){public void start(){setDaemon(true);super.start();}public void run(){while(true){checkOverflows();try{sleep(1000*20);}catch(InterruptedException inter){}}}};
/*void checkOverflows(){
System.out.println("Checking overflows");
x%=Rad360;y%=Rad360;z%=Rad360;
//??????????????
}*/
/**
* If you want to change all values use this
* @param newYaw
* @param newPitch
* @param newRoll
*/
public void resetYawPitchRoll(double newYaw,double newPitch,double newRoll){
x=newPitch;
y=newYaw;
z=newRoll;
//Shall we avoid overflows here or in a Thread? Here we want speed
//x%=Rad360;y%=Rad360;z%=Rad360;
//System.out.println("Yaw:"+Math.toDegrees(y));
//System.out.println("Pitch:"+Math.toDegrees(x));
//System.out.println("Roll:"+Math.toDegrees(z));
x/=2;
y/=2;
z/=2;
//now we reset the total Quat
q.set(0,0,0,1);
//now set the current Quat for each axis
qx.w=Math.cos(x);
qx.x=Math.sin(x);
qy.w=Math.cos(y);
qy.y=Math.sin(y);
qz.w=Math.cos(z);
qz.z=Math.sin(z);
//Result of all angles in main quat
//THIS ORDER MUST NOT BE CHANGED !!!!!!!!!!!!!!
q.mul(qx);
q.mul(qy);
q.mul(qz);
//next version try speed up all multiplications
trans.set(q);
}
/**
* Adding (or subtracts if negative) those Radians to the current Yaw,Pitch,Roll radians
* @param difYaw
* @param difPitch
* @param difRoll
*/
public void addYawPitchRoll(double difYaw,double difPitch,double difRoll){
dx=difPitch;
dy=difYaw;
dz=difRoll;
dx/=2;
dy/=2;
dz/=2;
x+=dx;
y+=dy;
z+=dz;
//now set the STEP -dif Quat for each axis
qx.w=Math.cos(dx);
qx.x=Math.sin(dx);
qy.w=Math.cos(dy);
qy.y=Math.sin(dy);
qz.w=Math.cos(dz);
qz.z=Math.sin(dz);
//Multipling new values with the existing MAIN QUAD
//THIS ORDER MUST NOT BE CHANGED !!!!!!!!!!!!!!
q.mul(qx);
q.mul(qy);
q.mul(qz);
//next version try speed all multiplications
trans.set(q);
}
public void addYaw(double difYaw){
dy=difYaw;
dy/=2;
y+=dy;
qy.w=Math.cos(dy);
qy.y=Math.sin(dy);
q.mul(qy);
trans.set(q);
}
public void addPitch(double difPitch){
dx=difPitch;
dx/=2;
x+=dx;
qx.w=Math.cos(dx);
qx.x=Math.sin(dx);
q.mul(qx);
trans.set(q);
}
public void addRoll(double difRoll){
dz=difRoll;
dz/=2;
z+=dz;
qz.w=Math.cos(dz);
qz.z=Math.sin(dz);
q.mul(qz);
trans.set(q);
}
public void addYaw(){
y+=dy;
q.mul(qyAdd);
trans.set(q);
}
public void addPitch(){
x+=dx;
q.mul(qxAdd);
trans.set(q);
}
public void addRoll(){
z+=dz;
q.mul(qzAdd);
trans.set(q);
}
public void addYawMinus(){
y-=dy;
q.mul(qyMinus);
trans.set(q);
}
public void addPitchMinus(){
x-=dx;
q.mul(qxMinus);
trans.set(q);
}
public void addRollMinnus(){
z-=dz;
q.mul(qzMinus);
trans.set(q);
}
public void setAddStepsForYawPitchRoll(double difYaw,double difPitch,double difRoll){
dx=difPitch;
dy=difYaw;
dz=difRoll;
dx/=2;
dy/=2;
dz/=2;
//preparing Adding steps
qxAdd.w=Math.cos(dx);
qxAdd.x=Math.sin(dx);
qyAdd.w=Math.cos(dy);
qyAdd.y=Math.sin(dy);
qzAdd.w=Math.cos(dz);
qzAdd.z=Math.sin(dz);
//preparing the Minus steps
qxMinus.w=Math.cos(-dx);
qxMinus.x=Math.sin(-dx);
qyMinus.w=Math.cos(-dy);
qyMinus.y=Math.sin(-dy);
qzMinus.w=Math.cos(-dz);
qzMinus.z=Math.sin(-dz);
}
}
Δεν υπάρχουν σχόλια:
Δημοσίευση σχολίου