func (r *Receiver) SetCarTransform(dt float64) {
p := r.Car.Center
up := glm.Vec3d{0, 0, 1}
front := glm.Vec4d{0, 1, 0, 0}
// u := glm.Vec3d{r.Car.Direction[0],r.Car.Direction[1],r.Car.Direction[2]}
rot := RotationBetweenNormals(front, r.Car.Direction)
m := glm.Translate3Dd(p[0], p[1], p[2]).Mul4(rot)
r.Data.Car.Transform = m
fwav := r.Car.FrontWheelAngularDeviation
rwav := r.Car.RearWheelAngularDeviation
fr := glm.HomogRotate3DXd(-fwav * 180 / math.Pi)
rr := glm.HomogRotate3DXd(-rwav * 180 / math.Pi)
wheel, ok := r.Data.Car.FindModelWithName("Wheel_BackLeft")
if ok {
wheel.Transform = wheel.BaseTransform.Mul4(rr)
}
wheel, ok = r.Data.Car.FindModelWithName("Wheel_BackRight")
if ok {
wheel.Transform = wheel.BaseTransform.Mul4(rr)
}
wheel, ok = r.Data.Car.FindModelWithName("Wheel_FrontLeft")
ft := glm.HomogRotate3Dd(-r.Car.FrontWheelO, up)
if ok {
wheel.Transform = wheel.BaseTransform.Mul4(ft).Mul4(fr)
}
wheel, ok = r.Data.Car.FindModelWithName("Wheel_FrontRight")
if ok {
wheel.Transform = wheel.BaseTransform.Mul4(ft).Mul4(fr)
}
}
func NodeTransform(node *collada.Node) glm.Mat4d {
transform := glm.Ident4d()
for _, matrix := range node.Matrix {
v := matrix.F()
transform = transform.Mul4(glm.Mat4d{
v[0], v[1], v[2], v[3],
v[4], v[5], v[6], v[7],
v[8], v[9], v[10], v[11],
v[12], v[13], v[14], v[15],
}.Transpose())
}
for _, translate := range node.Translate {
v := translate.F()
transform = transform.Mul4(glm.Translate3Dd(v[0], v[1], v[2]))
}
for _, rotation := range node.Rotate {
v := rotation.F()
transform = transform.Mul4(glm.HomogRotate3Dd(v[3]*math.Pi/180, glm.Vec3d{v[0], v[1], v[2]}))
}
for _, scale := range node.Scale {
v := scale.F()
transform = transform.Mul4(glm.Scale3Dd(v[0], v[1], v[2]))
}
return transform
}
func RotationBetweenNormals(n1, n2 glm.Vec4d) glm.Mat4d {
axis := Cross3D(n1, n2)
dot := n1.Dot(n2)
if !NearZero(axis) {
angle := math.Acos(dot)
return glm.HomogRotate3Dd(angle, axis.Normalize())
} else if dot < 0 {
for e := 0; e < 3; e++ {
v := glm.Vec4d{}
v[e] = 1
cross := Cross3D(n1, v)
if !NearZero(cross) {
return glm.HomogRotate3Dd(math.Pi, cross.Normalize())
}
}
panic(fmt.Sprintln("no orthogonal axis found for normal", n1))
}
return glm.Ident4d()
}
func Rotation(angle float64, axis glm.Vec4d) glm.Mat4d {
return glm.HomogRotate3Dd(angle, V3(axis))
}
func (r *Receiver) LoadScene(filename string) {
doc, err := collada.LoadDocument(filename)
panicOnErr(err)
index, err := gtk.NewIndex(doc)
panicOnErr(err)
r.SceneIndex = index
model := gtk.EmptyModel("scene")
switch doc.Asset.UpAxis {
case collada.Xup:
case collada.Yup:
case collada.Zup:
model.Transform = glm.HomogRotate3DXd(-90).Mul4(glm.HomogRotate3DZd(90))
}
portalPattern, _ := regexp.Compile("^Portal_(\\d+)_(\\d+)")
geometryTemplates := make(map[collada.Id][]*gtk.Geometry)
for id, mesh := range r.SceneIndex.Mesh {
geoms := make([]*gtk.Geometry, 0)
for _, pl := range mesh.Polylist {
matches := portalPattern.FindStringSubmatch(mesh.VerticesId)
if matches == nil {
elements := make([]*gtk.DrawElements, 0)
drawElements := gtk.NewDrawElements(pl.TriangleElements, gl.TRIANGLES)
if drawElements != nil {
elements = append(elements, drawElements)
}
geometry := gtk.NewGeometry(string(id), pl.VertexData, pl.NormalData, elements)
geoms = append(geoms, geometry)
} else {
fmt.Println("ignoring Portal")
}
}
if len(geoms) > 0 {
geometryTemplates[id] = geoms
}
}
portalLink := map[int]int{}
portals := map[int]portal.Quad{}
for _, node := range r.SceneIndex.VisualScene.Node {
matches := portalPattern.FindStringSubmatch(node.Name)
transform := r.SceneIndex.Transforms[node.Id]
if matches == nil {
geoms := make([]*gtk.Geometry, 0)
for _, geoinstance := range node.InstanceGeometry {
geoid, _ := geoinstance.Url.Id()
geoms = append(geoms, geometryTemplates[geoid]...)
}
if len(geoms) > 0 {
child := gtk.NewModel(node.Name, []*gtk.Model{}, geoms, transform)
model.AddChild(child)
}
} else {
index, err := strconv.Atoi(matches[1])
if err != nil {
panic(err)
}
exit, err := strconv.Atoi(matches[2])
if err != nil {
panic(err)
}
mt := model.Transform.Mul4(transform)
center := mt.Mul4x1(glm.Vec4d{0, 0, 0, 1})
normal := mt.Mul4x1(glm.Vec4d{0, 0, 1, 0}).Normalize()
planev := mt.Mul4x1(glm.Vec4d{1, 0, 0, 0}).Normalize()
scale := glm.Vec4d{}
n := 0
for i := 0; i < 3; i++ {
sum := 0.0
for j := 0; j < 3; j++ {
sum += float64(mt[n] * mt[n])
n++
}
n++
scale[i] = math.Sqrt(sum)
}
quad := portal.Quad{
center,
normal,
planev,
scale,
}
portalLink[index] = exit
portals[index] = quad
// fmt.Println("portal node", node.Name, quad)
}
}
r.Data.Scene.AddChild(model)
for id, quad := range portals {
exit, ok := portals[portalLink[id]]
if ok {
up := portal.Cross3D(exit.Normal, exit.PlaneV)
// fmt.Println("pre ",exit)
exit = exit.Apply(glm.HomogRotate3Dd(math.Pi, up))
// fmt.Println("post",exit)
_, inverse, pva, _ := portal.PortalTransform(quad, exit)
portal := portal.Portal{quad, inverse, pva}
r.Portals = append(r.Portals, portal)
} else {
fmt.Println("no exit for portal", id, portalLink[id])
}
}
}